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Abstract 

Dynamic  obstacle  avoidance  is  an  important,  ubiquitous,  and  often  challenging  prob¬ 
lem  for  autonomous  mobile  robots.  This  thesis  presents  a  new  method  to  guarantee 
collision  avoidance  with  respect  to  moving  obstacles  that  have  constrained  dynamics 
but  move  unpredictably.  Velocity  Obstacles  have  been  widely  used  to  plan  trajectories 
that  avoid  collisions  with  obstacles  under  the  assumption  that  the  path  of  the  objects 
are  either  known  or  can  be  accurately  predicted  ahead  of  time.  However,  for  real 
systems,  this  predicted  path  will  typically  only  be  accurate  over  short  time-horizons. 
To  achieve  safety  over  longer  time  periods,  the  method  introduced  here  instead  con¬ 
siders  the  set  of  all  reachable  points  by  an  obstacle  assuming  that  the  dynamics  fit 
the  unicycle  model,  which  has  known  constant  forward  speed  and  a  maximum  turn 
rate  (sometimes  called  the  Dubins  car  model). 

This  thesis  extends  the  Velocity  Obstacle  formulation  by  using  reachability  sets 
in  place  of  a  single  “known”  trajectory  to  find  matching  constraints  in  velocity  space, 
called  Velocity  Obstacle  Sets.  The  Velocity  Obstacle  Set  for  each  obstacle  is  equivalent 
to  the  union  of  all  velocity  obstacles  corresponding  to  any  dynamically  feasible  future 
trajectory,  given  the  obstacle’s  current  state.  This  region  remains  bounded  as  the 
time  horizon  is  increased  to  infinity,  and  by  choosing  control  inputs  that  lie  outside  of 
these  Velocity  Obstacle  Sets,  it  is  guaranteed  that  the  host  agent  can  always  actively 
avoid  collisions  with  the  obstacles,  even  without  knowing  their  exact  future  paths. 
It  thus  follows  that,  subject  to  certain  initial  conditions,  an  iterative  planner  under 
these  constraints  guarantees  safety  for  all  time. 

Finally,  the  an  iterative  planner  is  repeatedly  tested  and  analyzed  in  simulation 
under  various  conditions.  If  the  time  horizon  is  set  to  some  finite  value,  the  guaranteed 
collision  avoidance  is  lost,  but  the  planned  trajectories  generally  become  more  direct. 
This  effect  of  varying  this  time  scale  also  depends  on  the  presence  of  static  obstacles 
in  the  environment  and  on  the  dynamic  limitations  of  the  host  robot. 
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Chapter  1 


Introduction 

1.1  Motivation 

Dynamic  obstacle  avoidance  is  an  important,  ubiquitous,  and  often  challenging  prob¬ 
lem  for  autonomous  mobile  robots.  The  characteristics  of  specific  scenarios  call  for 
different  sets  of  assumptions  and  different  collision  avoidance  algorithms. 

In  situations  with  long  time-scales  and  significant  uncertainty  about  the  future, 
accurately  generating  an  entire  plan  for  the  host  robot  at  once  may  not  be  feasible. 
Instead,  on-line  motion  planners  can  be  used  to  create  or  modify  the  trajectory  as 
new  information  becomes  available.  The  complexity  of  the  problem  often  prohibits 
these  approaches  from  being  able  to  guarantee  finding  a  path  to  the  goal.  Instead, 
these  planners  iteratively  extend  the  trajectory  along  the  most  promising  segments, 
while  ideally  maintaining  safety  with  respect  to  the  obstacles  in  the  environment. 

Safety  guarantees  for  such  planners  are  most  commonly  achieved  by  defining  safe 
states  and  restricting  the  planner  to  work  only  with  trajectories  composed  of  such 
states.  Safe  states  are  those  that  do  not  violate  the  imposed  collision  constraints  and 
can  transition  into  another  safe  state.  For  example,  in  structured  environments  in 
which  the  other  vehicles  practice  reasonable  collision  avoidance,  coming  to  a  complete 
stop  and  staying  stationary  can  be  considered  reaching  an  invariant  safe  state  (which 
can  be  propagated  indefinitely) ,  so  any  state  that  can  make  this  transition  would  be 
deemed  safe  [1,  2],  However,  if  the  dynamic  obstacles  exhibit  unpredictable  behavior , 
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as  often  is  the  case  in  many  real- world  environments,  it  becomes  much  more  difficult 
to  define  sufficient  conditions  that  allow  safe  states  to  be  propagated  forward  in  time. 
The  host  vehicle  may  be  struck  by  other  vehicles  if  it  stays  at  rest,  or  it  may  become 
surrounded  by  multiple  other  vehicles  such  that  collision  becomes  inevitable. 

Other  popular  collision  avoidance  approaches  include  being  able  to  accurately 
anticipate  imminent  collisions  such  that,  with  an  understanding  of  the  dynamical 
capabilities  of  the  host  robot,  timely  evasive  actions  may  be  taken.  However,  with 
multiple  unpredictable  robots,  forecasting  situations  like  becoming  from  which  colli¬ 
sions  are  not  immediate  but  eventually  inevitable  (like  becoming  surrounded)  is  not 
straight-forward,  and  formulations  like  the  collision-cone  approach  [3]  cannot  guar¬ 
antee  proper  handling  of  these  problematic  cases. 

This  thesis  presents  a  method  for  finding  velocity-space  constraints  for  an  on-line 
planner  that  guarantees  infinite  horizon  safety  in  an  environment  with  multi¬ 
ple  obstacles  that  have  constrained  dynamics  but  can  move  unpredictably, 
a  scenario  that  has  so  far  been  difficult  for  techniques  in  the  existing  literature.  This 
safety  guarantee  is  achieved  by  combining  the  reachability  set  as  a  function  of  time 
for  objects  subject  to  these  dynamics  [4]  with  the  velocity  obstacle  concept  [5]. 

1.2  Problem  Statement 

The  goal  is  to  safely  navigate  a  single  host  robot  through  a  2D  environment  with 
multiple  dynamic  obstacles.  Each  obstacle  moves  with  unicycle  dynamics  (sometimes 
referred  to  as  Dubins  dynamics);  it  has  a  fixed  forward  speed  v,  and  it  can  turn  at 
up  to  some  maximum  rate  uj: 


aj=^[cos(0)  sin(d)]T, 
\e\  <  uj, 
v  =  0. 
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Equivalently,  the  obstacle  can  be  described  as  having  a  minimum  turning  radius 
p  =  v/uj.  The  host  robot  may  be  subject  to  arbitrary  dynamic  constraints.  All  the 
vehicles  are  discs,  and  a  collision  occurs  between  if  the  distance  between  two  vehicles 
is  less  than  collision  radius  r.  Given  the  values  of  these  parameters  and  the  ability  to 
measure  exactly  the  current  location  and  orientation  of  all  dynamic  obstacles,  the  host 
robot  must  travel  in  such  a  way  that  it  will  never  come  in  contact  with  an  obstacle, 
without  any  additional  information.  The  host  robot  is  given  a  series  of  way-points 
that  it  attempts  to  reach,  if  it  can  do  so  without  jeopardizing  its  safety. 

1.3  Previous  Work 

An  expansive  collection  of  path  planning  algorithms  has  been  built  as  varied  tech¬ 
niques  geared  towards  a  gamut  of  problem  statements  have  been  designed,  modified, 
and  combined.  [6-11]  This  section  will  briefly  cover  the  existing  approaches  that  are 
most  closely  related  to  the  planning  problem  posed  in  this  thesis.  To  the  best  of  our 
knowledge,  there  is  no  solution  in  the  current  literature  that  gives  infinite  horizon 
safety  in  the  presence  of  unpredictable  obstacles. 

1.3.1  Velocity  Obstacle  Based  Formulations 

Using  the  original  formulation  of  the  velocity  obstacle  [5],  one  can  End  single  veloc¬ 
ity  trajectories  that  are  guaranteed  collision-free,  given  the  exact  trajectory  of  the 
obstacles  for  some  time-scale.  This  time-scale  could  be  infinite,  but  that  would  un¬ 
realistically  require  that  all  obstacle  trajectories  be  known  perfectly  for  all  time.  For 
some  applications  in  predictable  environments  [12-14],  this  is  an  effective  means  of 
collision  avoidance,  but  it  is  unsuitable  for  guaranteeing  long  term  safety  with  re¬ 
spect  to  unpredictable  obstacles.  Nonetheless,  this  work  introduces  the  idea  of  using 
velocity-space  constraints  to  concisely  represent  collision  avoidance  conditions,  which 
plays  a  central  role  in  the  algorithm  derived  in  this  thesis  and  is  reviewed  in  detail  in 
Section  2.2. 

Various  extensions  and  modifications  to  the  original  velocity-obstacle  approach 
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have  been  made.  Instead  of  assuming  a  known  infinite  trajectory,  in  [15],  the  motion 
of  the  obstacles  are  predicted  for  a  finite  duration  with  an  associated  uncertainty. 
Velocity  obstacles  are  computed  after  growing  the  obstacles  by  the  uncertainty,  and 
then  used  to  plan  safe  finite  segments.  Since  the  time-scale  is  finite,  there  is  no 
guarantee  that  a  trajectory  can  be  continued  safely  when  a  re-plan  is  necessary. 
Further,  according  to  the  authors  of  Ref.  [15],  growing  obstacles  by  the  uncertainty 
often  results  in  all  reachable  velocities  creating  potential  collisions.  Various  metrics 
can  be  used  to  select  a  velocity  with  relatively  low  likelihood  of  collision,  but  this 
would  still  be  unacceptable  if  safety  is  to  be  guaranteed. 

In  [16],  the  authors  appeal  to  a  safe  state  argument.  They  use  the  dynamics  of 
the  host  robot  and  of  each  obstacle  to  calculate  an  optimal  time  horizon,  such  that 
inevitable  collision  states  [17]  are  avoided  by  heeding  the  bounds  of  the  corresponding 
finite-time  velocity  obstacle.  Firstly,  this  method  still  requires  the  trajectories  of  the 
dynamic  obstacles  be  known  at  least  up  to  some  finite  horizon,  but  this  information 
is  not  always  available  in  the  most  general  settings.  Further,  it  is  argued  that  “any 
velocity  that  does  not  penetrate  this  [finite,  optimal  time  horizon]  velocity  obstacle 
should  allow  sufficient  time  under  the  given  control  authority  to  avoid  collision.’'  But 
this  in  fact  fails  to  account  for  the  interaction  of  the  constraints  imposed  by  multiple 
obstacles.  The  extremal  trajectories  that  are  computed  to  dodge  collisions  with  an 
obstacle  may  interfere  with  neighboring  obstacles,  so  a  velocity  that  respects  the 
optimal  horizon  velocity  obstacle  of  one  object  may  in  fact  be  forced  to  collide  with 
a  different  obstacle.  Thus,  this  method  does  not  guarantee  infinite  horizon  safety. 

1.3.2  Various  other  formulations 

In  [3],  “collision  cones”  are  defined  to  predict  collisions  between  objects  of  arbitrary 
size  and  shape,  if  the  objects  maintain  their  current  velocities.  For  rigid  bodies,  it 
is  then  possible  to  compute  the  necessary  adjustments  to  the  velocity  of  the  host 
robot  such  that  anticipated  collision  is  averted.  Based  on  the  dynamic  capabilities  of 
the  robot,  avoidance  maneuvers  can  be  computed  such  that  it  is  always  possible  to 
remain  the  collision  cone.  However,  much  like  the  issue  with  [16]  discussed  earlier, 
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the  feasibility  of  these  maneuvers  is  guaranteed  only  for  single  obstacle  case;  the 
possibility  of  multiple  obstacles  imposing  constraints  that  cannot  be  simultaneously 
satisfied  is  not  explicitly  accounted  for.  This  problem  arising  from  multiple  obstacles 
cannot  be  solved  by  defining  a  single  entity  composed  of  the  collective  configurations 
of  all  the  obstacles,  since  their  independent  motions  would  violate  the  rigid  body 
assumption. 

Other  work  includes  [18],  in  which  safe,  analytic  trajectories  are  found  given  robot 
and  obstacle  dynamic  constraints,  but  only  on  a  finite  horizon  with  a  given  endpoints 
and  without  conditions  for  propagating  safety.  Thus,  this  algorithm  cannot  practically 
handle  large  planning  problems  with  long  time  horizons. 

In  [19-22],  guaranteed  safety  is  achieved  for  multi-agent  systems  in  which  all  the 
agents  use  the  same  collision  avoidance  policy  with  feasible  invariant  sets.  These 
frameworks  rely  on  having  coordinated  authority  over  all  agents  in  the  environment, 
and  therefore  cannot  handle  external  unpredictable  agents. 

1.4  Summary  of  Contributions 

This  thesis  develops  a  framework  for  guaranteeing  infinite  horizon  collision  avoidance 
that  does  not  rely  on  any  prediction  of  the  obstacles’  motion.  This  is  done  by  map¬ 
ping  the  reachable  sets  of  the  dynamic  obstacles  into  velocity  space  to  form  Velocity 
Obstacle  Sets  which  are  then  used  to  define  safe,  invariant  single- velocity  trajectories. 

The  safety  guarantee  is  analytically  derived  and  proved,  and  the  resulting  iterative 
planner  is  thoroughly  demonstrated  in  various  forms  under  different  simulated  condi¬ 
tions.  It  is  also  shown  that  alternative  finite  horizon  velocity  obstacle  set  formulations 
may  be  implemented  as  reactive  planners  that  lack  long-term  safety  guarantees  but 
may  be  better  suited  for  certain  tasks,  depending  on  the  characteristics  of  the  host 
vehicle. 
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1.5  Thesis  Overview 


The  remainder  of  this  thesis  is  organized  as  follows:  Chapter  2  reviews  and  builds 
upon  key  concepts  from  existing  literature  that  are  critical  to  the  new  velocity  ob¬ 
stacle  set  algorithm.  These  topics  included  reachability  sets,  velocity  obstacles,  and 
invariant  safe  states.  Chapter  3  defines  and  derives  velocity  obstacle  sets.  A  method 
is  presented  for  efficiently  computing  a  concise  representation  of  constraints  as  a 
function  of  the  current  obstacle  states.  Safety  guarantees  on  the  infinite  horizon  are 
then  discussed  in  Chapter  4.  Chapter  5  describes  how  the  velocity  obstacle  sets  can 
be  used  in  iterative  planners.  The  behaviors  of  a  basic  infinite-horizon  planner  are 
demonstrated  in  simulation,  and  the  use  of  infinite  horizon  planners  and  finite  horizon 
planners  of  various  lengths  are  then  thoroughly  compared  for  a  variety  of  scenarios. 
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Chapter  2 


Background 

2.1  Reachability  and  Collision  Regions 

This  section  will  find  the  boundaries  of  regions  in  physical  space  that  may  result  in  a 
collision  with  a  given  obstacle,  as  a  function  of  time.  For  example,  if  the  location  of 
a  circular  obstacle  is  known  for  some  future  time,  the  collision  region  at  that  time  is 
a  circle  centered  on  the  known  obstacle  location,  with  radius  equal  to  the  sum  of  the 
obstacle  and  host  vehicle  radii.  If  the  center  of  the  host  vehicle  is  within  this  region 
at  that  time,  there  will  be  contact  with  the  given  obstacle.  These  regions  of  physical 
space  to  avoid  will  be  converted  into  velocity  space  constraints  in  Chapter  3. 

2.1.1  Reachability 

If  it  is  not  possible  to  know  exactly  the  location  of  the  obstacle  as  a  function  of  time, 
the  reachable  set  can  be  used  instead  to  generate  the  collision  region.  This  is  the 
union  of  all  possible  locations  of  the  obstacle  at  some  given  time.  By  avoiding  the 
entire  reachable  set  of  the  obstacle,  the  host  robot  avoids  any  possible  contact. 

In  [4],  the  authors  find  “the  set  of  all  possible  positions”  for  a  particle  that  “moves 
in  the  plane  with  constant  speed  and  subject  to  an  upper  bound  on  the  curvature 
of  its  path.”  These  dynamics  match  the  standard  unicycle  model  used  here,  and 
the  results  can  be  directly  applied  to  find  the  reachable  set  of  a  given  obstacle  as  a 
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Table  2.1:  Summary  of  important  terms  and  parameters  used. 


symbol 

Section 

Meaning 

VO 

2.2 

Velocity  obstacle:  constraint  in  velocity  space  given  tra¬ 
jectory  of  obstacle. 

VOS(t0,tf ) 

3.1 

Velocity  obstacle  set:  constraint  in  velocity  space  given 
reachability  of  obstacle.  Defined  over  a  time  window  as 
USVR  (t)Vte[t0,tf\. 

VOS(t0 ,  oo) 

3.3.2 

Infinite  horizon  VOS.  Defined  for  all  future  times. 

r 

4 

Time  horizon  of  VOS,  r  =  tf  —  t0. 

V 

1.2 

Constant  forward  speed  of  moving  obstacles. 

CO 

1.2 

Maximum  turn  rate  of  moving  obstacles. 

p 

1.2 

Minimum  turning  radius  of  obstacle,  p  = 

r 

1.2 

Collision  radius.  Sum  of  host  vehicle  radius  and  obstacle 
radius. 

CR  (t) 

2.1.2 

Collision  region:  reachable  set  grown  by  collision  radius, 
i.e.,  set  of  all  points  within  r  of  reachable  set. 

SCR  (t) 

2.1.2 

Simplified  collision  region:  expanded  version  of  CR(t)  for 
simplicity;  contains  all  points  that  could  contact  obstacle. 

Si, 2, 3, 4, 5(0,  t) 

2.1.2 

Arcs  that  parametrically  define  respective  segments  of 
boundary  of  SCR(f). 

SVR(f) 

3.1 

Simplified  velocity  region:  velocity  space  constraint  as¬ 
sociated  with  SCR(f);  SVR(f)  =  ^SCR(f). 

C(t ) 

3.1 

Boundary  of  SVR(f). 

Ql,2,3,4,5(0, 

3.1 

Arcs  that  parametrically  define  respective  segments  of 
C(t). 

n  |p(0) 

3.1 

Normal  vector  to  C{t)  at  point  P. 

Byos(t<htf) 

3.1 

Boundary  of  VOS(t0,tf ) 

St 

5.2.1 

Discrete  time-step  at  which  dynamics  are  propagated  in 
simulation. 

AT 

5.2.1 

Interval  at  which  re-planning  occurs. 

Ad 

5.2.2 

Maximum  instantaneous  change  of  robot  heading  allowed 
at  each  re-plan. 

^ max 

5.2.2 

Equivalent  turn-rate  constraint  of  host  vehicle  repre¬ 
sented  by  imposing  limits  on  A 6 
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Reachable  Set  of  an  point  mass:  v  —  lm/s,  p  —  l(Jm,  t  —  20  s 


Figure  2-1:  Example  reachable  set  for  v  =  lm/s,p  =  10m,  t  =  20s.  The  reachable  region  is 
bound  by  segments  of  the  4  curves  A\,  A2,  B\,  B2-  Corresponding  trajectories  to  points  on 
these  curves  are  shown  in  dashed  lines. 

function  of  time.  Without  loss  of  generality,  consider  an  obstacle  P  that  starts  at  the 
origin  with  heading  (clockwise  angle  measured  from  the  y  axis)  9  =  0  at  time  t  =  0. 
P  moves  with  constant  speed  v  and  maximal  turn  rate  w. 

x\ p  =  n[sin(6l)  cos  (9)}T , 

\9\  <  u, 

v  =  0. 

Equivalently,  P  has  a  minimal  turning  radius  p  =  v/u. 

It  is  shown  in  [4]  that  the  reachable  region  at  time  t  is  bounded  by  4  parametric 
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curves  (Figure  2-1).  A\2  are  given  by 


A,(9,t) 


a2(0,  t) 


— p(l  —  cos 9)  +  (vt  +  p9 )  sin# 
—p  sin  9  +  (vt  +  p6 )  cos  9 

p(  1  —  cos#)  +  (vt  —  p9)  sin  9 
p  sin  9  +  (vt  —  p9)  cos  9 


(2.1) 


(2.2) 


with  —  wt  <  9  <  0  for  Ai  and  0  <  9  <  wt  for  A2.  The  points  on  these  boundaries 
correspond  to  the  minimal-time  Dubins  paths  [23]  of  maximal-rate  turn  followed  by 
driving  straight  (black  and  blue  dashed  lines  in  Figure  2-1).  Similarly,  Bi>2  are 


Bi(^,t) 

B2(il>,t) 


—p( 2  cos  0  —  1  —  cos(20  —  wt)) 
p(— 2sin0  —  sin(— 2-0  —  wt)) 

p(  2  cos  0  —  1  —  008(20  —  wt)) 
p(  2sin0  —  sin(20  —  wt)) 


(2.3) 


(2.4) 


with  —0*  <  0  <  0  for  i?i  and  0  <  0  <  0*  for  i?2-  0*  is  found  by  solving 


2  cos  0  *  —  1  —  cos(20  *  —  wt)  =  0. 


The  points  on  these  boundaries  correspond  to  paths  of  maximal-rate  turn  in  one 
direction  followed  by  maximal-rate  turn  in  the  other  direction  (red  and  magenta 
dashed  lines  in  Figure  2-1). 

Given  the  initial  position  and  orientation  of  a  moving  obstacle  that  follows  the 
described  dynamics,  its  location  at  any  given  future  time  lies  within  these  bounds. 


2.1.2  Simplified  Collision  Region 

Equations  (2.1)-(2.4)  describe  the  possible  locations  of  the  obstacle  for  a  given  time 
t.  This  region  is  grown  by  the  collisio7i  radius  r  to  find  the  set  of  locations  at  which 
the  robot  could  be  in  contact  with  the  obstacle  at  time  t.  For  example,  with  a  circular 
robot  of  radius  rq  and  a  circular  obstacle  of  radius  r2,  r  =  rq  +  r2.  As  long  as  the 
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Collision  regions  at  t  =  20 


Figure  2-2:  Example  simplified  collision  region.  The  region  from  Figure  2-1  is  grown  by 
the  collision  radius  to  produce  CR(f),  in  light  blue.  CR(f)  is  then  expanded  using  S5  into 
SCR(f),  in  light  yellow,  for  simplicity. 

robot  is  outside  of  this  grown  collision  region  CR (t)  (light  blue  in  Figure  2-2)  at  time 
t,  there  cannot  be  a  collision  with  the  obstacle  at  the  specified  instant  in  time. 

To  simplify  the  representation  of  the  various  segments  bounding  the  collision  re¬ 
gion,  segment  A5  (dashed  red  line  in  Figure  2-2)  is  introduced  to  replace 
(dashed  magenta  line).  This  expands  the  original  reachable  set  to  produce  a  new 
region  that  contains  the  original  set.  (See  Section  2.1.3  for  discussion  of  why  this  is 
a  safe  simplification).  Expanding  this  simplified  reachable  set  by  collision  radius  r 
produces  the  simplified  collision  region  (SCR),  with  boundaries  Si,...,5.  This  region 
is  defined  for  each  obstacle  at  a  specific  time,  written  SCRj(t),  where  i  indexes  the 
obstacles. 

Figure  2-2  shows  that  the  boundary  of  the  simplified  collision  region  for  a  given 
instant  in  time  is  a  combination  of  up  to  5  possible  segments,  Si, 2, 3, 4(0,  t)  and  S5(t). 
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Four  of  these  segments  are  parameterized  in  9,  which  is  the  direction  of  the  normal 
vector  as  measured  from  the  y  axis,  ranging  from  —  n  to  n.  That  is,  at  any  point  on 
<Si,2,3,4 (9,t),  the  normal  unit  vector  n  is  given  by 


n(9) 


sin  0 
cos  9 


(2.5) 


This  equation  is  easily  confirmed  by  checking  that  the  normal  vector  is  orthogonal  to 
the  tangent: 


J  =  1, ... ,4. 


(2.6) 


Si, 2(9,  t)  (solid  blue  in  Figure  2-2)  are  derived  from  Ai^(9,t)  by  adding  r  to  Equa¬ 
tion  2.1  and  Equation  2.2  along  the  outward  (with  respect  to  the  interior  of  the 
reachable  set)  normal.  They  are  parametrically  represented  as 


Si(9,t) 
S2(9,t) 

with  respective  domains 

Si: 

S2: 


—p(  1  —  cos  9)  +  (vt  +  p9  +  r)  sin  9 
—p  sin  9  +  (vt  +  p9  +  r)  cos  9 

p(  1  —  cos  9)  +  (vt  —  p9  +  r)  sin  9 
p  sin  9  +  (vt  —  p9  +  r)  cos  9 

max(— wt,  — 7r)  <  9  <  0, 

0  <  9  <  min  (wt,  1 r). 


(2.7) 


(2.8) 


(2.9) 

(2.10) 


S3, 4,(9,  t)  (solid  green  in  Figure  2-2)  are  circular  arcs  of  radius  r  that  wrap  around 
the  left  and  right  bottom  corners  of  the  reachable  set.  The  centers  of  these  arcs  are 
found  by  substituting  max(— 7 r,—wt)  and  max(7T, wt)  into  Equation  2.1  and  Equa¬ 
tion  2.2  respectively.  If  \wt\  >  1 r,  then  it  is  clear  that  S\,S2,  and  S5  enclose  the 
simplified  collision  region.  This  is  both  geometrically  intuitive  (Figure  2-2)  and  rigor¬ 
ously  addressed  (Section  2.1.3).  When  \wt\  <  1 r,  the  parametric  expressions  for  these 
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arcs  are 


63  (M) 


s4(e,t ) 


— p(l  —  cos(cuf))  +  rsin# 
psin(cuf)  +  r  cos  9 

p(  1  —  cos(cuf))  +  r  sind 
p  sin  (cut)  +  r  cos  9 


with  respective  domains 


S3  :  —  vr  <  9  <  —cut, 

64  :  cut  <  9  <  7T. 


(2.11) 


(2.12) 


(2.13) 

(2.14) 


Note  that  for  t  >  534  are  no  longer  part  of  the  boundary  of  SCR,;(t). 

Finally,  65(f)  is  the  horizontal  line  segment  joining  the  lowest  points  in  S  1,2, 34(^5 1). 
These  are  either  63U 7T,  t)  and  ^(vr,  t),  or  S\(— n,  t)  and  ^(vr,  t),  depending  on  which 
pair  of  curves  is  defined  for  6  =  ±7r  at  time  t.  For  this  segment,  the  outward  normal 
unit  vector  is  [0  —  1]T- 

*5*1, 2, 3, 4(^5 1)  can  be  combined  into  a  piecewise  curve  R(9,t )  for  which  0  e  [ — 7r,  7t] . 
The  region  below  R  and  above  S5  defines  the  simplified  collision  region  SCRj(f)  for 
obstacle  i  at  any  time  t.  For  clarity,  specific  segments  Si(9,t )  will  be  referred  to 
instead  of  R{9 ,  t )  as  a  whole. 


2.1.3  Validity  of  Using  SCR(t)  in  Place  of  CR(t) 

Guaranteed  collision  avoidance  is  maintained  when  replacing  the  collision  region 
CR(f)  with  SCR(f),  if  SCR(f)  includes  all  of  CR(t).  Since  65(f)  is  the  new  lower 
boundary  of  the  region,  this  inclusion  is  true  if  no  points  on  the  original  boundary 
of  CR(f)  lie  below  65(f).  Equivalently,  one  must  show  that  defining  A5(t)  to  join 
A\{— ut,  t)  and  A2(ut,t)  as  the  new  lower  boundary  does  not  exclude  points  from 
the  original  region,  i.e.,  no  points  in  the  replaced  boundary  segments  ,  -6(1,2}  lie 
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below  A5(t)  (see  figure  2-3).  That  is, 


A{h2hy(9,t)>A5jy(t)  \/\9\>7i  (2.15) 

and 

£{i,2},2,(tM)  >  A5iy(t)  V|-0|  <  V7*-  (2-16) 

Equation  2.15  is  only  a  concern  for  cot  >  7r  (because  0  <  cot),  and  within  this 
domain,  A^y{9,t)  =  A2>y(7r,t).  Using  the  first  and  second  derivatives  of  A2}V(9,t)  (see 
Equation  2.2)  with  respect  to  t,  it  is  easy  to  show  that  A2tV  has  local  minimums  at 
6  =  {vt,37t,  . . .}.  Equation  2.2  evaluated  at  these  points  yields 

A2,y(0,t )  =  -(vt  -  p9 ), 

which  increases  with  9.  Clearly,  the  global  minimum  occurs  at  9  =  n.  Hence,  Equa¬ 
tion  2.15  is  satisfied. 

Starting  with  Equation  2.4, 

B2y{il ;,  t)  =  p(  2  sin^  —  sin(2,0  —  cot)), 
dB 

,  2’y  =  p(  2  cos  -0  —  2  cos(2^  —  cot)) 
dap 

=  B2tX(aJj,  t)  +  (1  -  cos(2 ajj  -  cot)) 

>  B2tX(a/j,t). 

From  Section  3.2  in  Ref.  [4],  B2tX(ap,t)  >  0  for  the  entire  domain  of  a/j.  Therefore, 

^>ov^[o,r]. 

Hence,  By  is  an  increasing  function  of  o/j,  and  since  and  A^y{t)  =  B2jy(0,t),  Equa¬ 
tion  2.16  is  satisfied. 

Therefore,  the  modified  reachability  set  is  a  valid  over-bound  of  the  exact  reach¬ 
ability  set,  so  the  corresponding  collision  region  SCR(f)  is  a  safe  over-bound  of  the 
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Collision  regions  at  t  —  oO 


Figure  2-3:  SCR(t  =  50).  SCR(t)  defined  using  S5  includes  all  of  the  original  exact  collision 
region  if  the  dotted  red  line  A 5  lies  below  A\y2  as  it  curves  inward  (as  time  is  extended) 
and  all  parts  of  B\^2.  The  proof  presented  in  Section  2.1.3  first  shows  that,  even  as  t  is 
increased  to  infinity  and  the  curves  A\2  are  extended,  while  they  periodically  loop  back 
down  towards  A$,  the  low  point  of  each  pass  is  higher  than  the  previous.  It  is  then  shown 
B\  2  increase  in  y  away  from  the  endpoints  that  joins  with  A\, 2,  hence  A5  also  lies  below 
all  of  B1i2. 


Figure  2-4:  SCR(f  =  120).  As  t  is  increased,  the  curves  A±}2  continue  indefinitely,  but  they 
can  be  safely  truncated  using  A 5  since  they  lie  completely  above  A$. 
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A 


▲ 


Figure  2-5:  Velocity  Obstacle  associated  with  a  circular,  stationary  obstacle  (red  circle  in 
top  frame).  Should  the  host  robot  (blue  point  at  the  origin  of  the  top  frame)  move  from  the 
origin  with  any  velocity  that  lies  within  the  velocity  obstacle  (green  cone  in  bottom  frame), 
it  will  eventually  collide  with  the  obstacle  at  some  future  time. 

exact  collision  region  CR(t). 


2.2  Velocity  Obstacles 

This  section  review  the  conversion  of  constraints  in  physical  space  to  constraints  in 
velocity  space.  The  authors  of  [5]  introduce  the  concept  of  velocity  obstacles. 

2.2.1  Basic  Picture 

Consider  an  obstacle  of  radius  r  at  initial  location  x.  Let  the  host  robot  be  a  point 
mass  starting  at  the  origin.  If  the  robot  drives  indefinitely  in  the  direction  of  this 
obstacle,  it  will  collide  with  the  obstacle  at  some  future  time.  That  is,  if  the  angle 
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Figure  2-6:  Velocity  Obstacle  associated  with  a  circular  obstacle  moving  along  a  single¬ 
velocity  trajectory.  Translating  the  original  velocity  obstacle  by  this  relative  velocity  yields 
the  new  velocity  obstacle.  Any  velocity  from  within  the  green  region  will  eventually  result 
in  contact  with  the  moving  obstacle. 

measured  from  the  positive  x  axis  of  a  single-direction  trajectory  lies  between  a(x)  ± 
arctan(r/|ir|),  this  trajectory  will  result  in  a  collision.  These  angular  bounds  translate 
directly  into  a  cone  in  velocity  space  (green  region  in  Figure  2-5)  whose  sides  are 
rays  pointing  along  these  critical  angles  and  whose  base  extends  out  to  infinity.  A 
velocity  within  these  bounds  (or,  a  velocity  from  within  this  region  of  velocity  space ) 
will  contact  the  obstacle  if  continued  indefinitely,  while  all  other  velocities  are  safe, 
as  long  as  the  velocity  is  never  changed. 

For  an  obstacle  that  moves  with  constant  velocity  v*,  the  same  results  apply 
as  relative  velocities  (see  Figure  2-6).  That  is,  a  collision  will  occur  if  and  only  if 
the  relative  velocity  between  the  host  robot  and  the  obstacle  lies  within  the  original 
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velocity  region  [24],  Therefore,  the  original  region  in  velocity  space  can  be  simply 
translated  by  v*,  and  this  produces  the  linear  velocity  obstacle ,  which  applies  to 
obstacles  moving  with  constant  velocities.  It  is  also  possible  to  compute  the  nonlinear 
velocity  obstacle  for  obstacles  moving  along  arbitrary  trajectories  (See  Figure  2-8); 
the  process  is  discussed  in  detail  in  [25]. 


2.2.2  Equivalent  Interpretation 

The  following  introduces  an  alternate  derivation.  The  results  of  this  approach  are 
identical  to  the  more  intuitive  case  from  2.2.1,  but  this  process  is  more  easily  adapted 
to  compute  velocity  obstacle  sets  (Chapter  3)  for  unpredictable  obstacles,  which  is 
the  main  result  of  this  thesis. 

Given  a  set  of  coordinates  X  (t  =  r)  that  must  not  be  entered  at  time  r  (collision 
region  of  a  physical  obstacle  at  a  given  time),  there  is  a  corresponding  set  of  velocities 


V{t  =  r) 


X(r) 


found  by  simply  dividing  X  by  r.  If  the  vehicle  were  to  start  at  the  origin  at  t  —  0 
and  take  a  linear,  single-velocity  trajectory  using  any  of  the  velocities  in  the  set 
V(t),  it  would  end  up  at  a  coordinate  inside  set  X  at  time  r.  Any  other  single- 
velocity  trajectory  would  successfully  stay  clear  of  this  constraint.  This  set  V(t)  of 
velocities  to  avoid,  associated  with  collisions  occurring  at  a  specific  future  time,  is  an 
instantaneous  velocity  obstacle  (blue  circles  in  Figure  2-7  and  red  circles  in  2-8). 

For  moving  and  stationary  obstacles  that  exist  for  longer  than  a  single  time  instant, 
the  host  vehicle  must  jointly  avoid  all  the  collision  regions  at  their  matching  time 
instants,  as  given  by  the  trajectory  of  the  obstacle.  The  union  of  these  resulting 
constraints  for  each  obstacle  forms  a  2D  shape  in  velocity  space.  This  defines  the 
velocity  obstacle 


VO{t0,tf)  =  \JV{t)  Vte[t0,t/] 


Figure  2-7:  Equivalent  interpretation  of  the  velocity  obstacle  for  moving  obstacle.  Instanta¬ 
neous  locations  (shades  of  red  in  top  frame)  of  the  obstacle  are  converted  into  velocity-space 
regions  (blue  circles  in  bottom  frame)  by  dividing  by  time.  The  union  of  these  regions  over 
a  time  interval  produce  (a  section  of)  the  same  velocity  obstacle  (green  cone)  described  in 
section  2.2.1.  Note  that  to  must  be  >  0. 

where  t0  >  0  and  tf  >  t0  bound  the  time  window  of  interest.  Single- velocity  trajecto¬ 
ries  using  velocities  from  inside  this  velocity  obstacle  would  intersect  the  constrained 
regions  at  some  time  within  the  specified  window.  If  the  obstacle  is  moving  with  fixed 
velocity  over  the  duration  of  interest,  this  results  in  the  linear  velocity  obstacle  (see 
Figure  2-7).  Ref.  [5]  shows  that  the  linear  velocity  obstacle  is  a  truncated  segment  of 
a  cone  that  expands  from  the  origin  towards  the  moving  object. 

Where  the  truncation  occurs  is  determined  by  the  time  window.  It  does  not  make 
sense  to  define  V(0),  because  contact  with  the  obstacle  at  t  —  0  is  simply  determined 
by  the  initial  conditions.  As  the  lower  time  bound  approaches  0,  the  open  end  of  the 
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cone  tends  towards  infinity,  as  collisions  in  the  very  near  future  would  entail  driving 
towards  the  obstacle  at  high  speeds.  When  these  speeds  exceed  dynamic  capacities  of 
the  host  vehicle,  there  are  effectively  no  longer  of  practical  interest,  and  the  velocity 
obstacle  can  be  truncated  at  some  small  initial  time.  As  the  upper  time  bound 
approaches  infinity,  the  sharp  end  of  the  cone  converges  to  the  obstacle’s  terminal 
velocity  (see  figure  2-7:  as  t  increases,  V(t)  shrinks  and  tends  towards  the  tip  of  the 
VO  cone). 

Nonlinear  velocity  obstacles  can  be  computed  for  objects  that  move  in  arbitrary 
ways  over  the  time  window.  Layering  the  velocity  constraints  generated  at  each  time 
instant  produces  a  warped  cone  (see  Figure  2-8),  and  the  calculation  and  representa¬ 
tion  of  this  shape  is  discussed  in  Ref.  [25].  Velocities  outside  of  this  region  generate 
safe,  single-velocity  trajectories;  the  safety  of  nonlinear  or  speed-varying  robot  trajec¬ 
tories  are  explored  in  [26]. 

The  standard  approach  for  defining  and  using  velocity  obstacles  in  the  existing 
literature  ([5,  15,  16,  24,  27])  requires  a  mapping  that  gives  the  location  of  the  object 
as  a  function  of  time  over  the  specified  window  in  order  to  produce  the  velocity 
obstacle.  This  typically  limits  the  horizon  to  accurately  predictable  time-scales  and 
requires  any  prediction  error  to  be  handled  by  growing  the  collision  size.  While 
there  has  been  extensive  work  in  improving  prediction  of  obstacle  motion  [28,  29], 
this  framework  is  nonetheless  generally  ill-suited  for  guaranteeing  long  term  collision 
avoidance.  This  thesis  extends  the  velocity  obstacle  concept  to  scenarios  in  which  the 
predicted  trajectory  is  unavailable. 

2.3  Invariant  Safe  States 

This  section  briefly  reviews  the  concept  of  invariant  safe  states,  which  is  often  used  to 
prove  extended  collision  avoidance.  This  material  will  be  relevant  to  the  guarantees 
of  collision  avoidance  (Chapter  4)  and  to  properties  of  iterative  planners  (Chapter  5). 

In  extended  horizon  scenarios,  it  is  often  only  possible  to  make  detailed  motion 
plans  into  the  near  future  without  explicitly  reaching  a  terminal  goal  state.  The 
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Non-linear  velocity  obstacle  for  object  moving  along  known  trajectory 
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Figure  2-8:  The  nonlinear  velocity  obstacle  for  an  object  moving  along  a  known  curved 
trajectory.  Note  that  the  physical  obstacle  and  its  path  are  sketched  and  super-imposed 
upon  the  velocity  space  coordinates.  This  image  is  only  for  visualization;  these  entities  are 
defined  in  physical  space  ( x  and  y,  as  opposed  to  vx  and  vy),  and  have  no  actual  meaning 
in  this  frame.  If  the  obstacle  trajectory  is  assumed  to  continue  indefinitely  at  some  fixed 
velocity,  the  tip  of  the  nonlinear  velocity  obstacle  terminates  at  the  coordinate  corresponding 
to  that  velocity,  and  if  the  obstacle  were  to  drive  in  circles,  the  tip  of  the  nonlinear  velocity 
obstacle  converges  to  the  origin;  this  is  the  behavior  of  the  host  vehicle  that  collides  with 
the  obstacle  in  the  very  distant  future. 


final  goal  may  be  far  out  of  reach  or  the  environment  may  be  too  dynamic,  so  a 
short  term  plan  is  executed,  bringing  the  robot  closer  to  the  goal  with  the  intention 
of  regenerating  more  short-term  plans  in  the  future.  Typically,  it  is  fairly  straight¬ 
forward  to  avoid  collisions  within  the  planning  horizon.  However,  it  is  often  difficult 
to  account  for  issues  that  may  become  unavoidable  farther  in  the  future.  That  is,  even 
with  the  ability  to  re-create  a  new  short-term  plan  before  the  horizon  of  the  current 
plan  expires,  decisions  from  the  current  plan,  combined  with  the  unpredictability 
of  the  environment,  may  make  it  impossible  to  find  a  safe  trajectory  at  the  next 
planning  iteration.  The  momentum  of  the  host  vehicle  may  make  an  evasive  maneuver 
impossible,  or  the  terminal  position  of  the  host  robot  may  be  in  the  path  of  a  moving 
obstacle. 
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To  cleanly  handle  such  scenarios,  many  iterative  path  planners  (such  as  the  ones 
in  [1]  and  [2]),  define  invariant  safe  states.  This  is  the  set  of  host  vehicle  states  such 
that 


•  all  safety  (collision  avoidance)  constraints  are  satisfied, 

•  it  is  dynamically  feasible  to  transition  back  into  itself. 

For  example,  in  static  environments,  staying  motionless  without  contacting  a  static 
obstacle  would  be  an  invariant  safe  state.  For  autonomous  vehicles  driving  in  an 
urban  setting  [1],  the  rules  of  the  road  make  stopping  in  certain  areas,  like  beside 
a  curb,  invariant  safe  states.  Such  a  state  can  be  maintained  indefinitely  if  need 
be.  This  means  that  if  a  finite-horizon  plan  terminates  in  such  a  state,  the  plan  can 
be  trivially  extended  for  any  amount  of  time  with  guaranteed  collision  avoidance. 
Furthermore,  any  state  that  can  transition  into  an  invariant  safe  state  can  also  be 
deemed  safe  and  considered  a  valid  terminal  condition  for  finite-trajectory  plans. 

Therefore,  if  an  iterative  planner  is  limited  to  choosing  finite-horizon  plans  that 
terminate  in  safe  states,  such  a  planner  is  guaranteed  to  avoid  collisions  for  arbitrary 
time  scales.  The  existence  of  at  least  one  feasible  option  every  time  a  new  plan  is 
computed  is  ensured,  as  the  planner  can  always  choose  to  transition  into  the  invariant 
safe  state  and  stay  there.  However,  nothing  can  be  said  about  the  existence  of  a 
solution  that  brings  the  robot  to  the  desired  goal;  enforcing  this  constraint  may 
in  fact  cause  the  planner  to  overlook  potential  trajectories  to  the  goal  in  favor  of 
trajectories  that  cling  in  proximity  to  the  invariant  safe  states.  This  issue  must  be 
dealt  with  on  a  case  to  case  basis,  depending  on  the  specific  goals  and  challenges  of 
each  application. 

In  relatively  unstructured  environments,  it  can  be  very  challenging  to  define  in¬ 
variant  safe  states.  The  scenario  at  the  core  of  this  thesis  involves  multiple  dynamic 
obstacles  whose  trajectories  are  unpredictable,  governed  only  by  their  known  dynam¬ 
ical  limitations.  The  classic  invariant  safe  states  of  bringing  the  host  vehicle  to  a  stop 
at  some  location  no  longer  apply,  as  it  could  still  be  possible  for  the  host  vehicle  to 
be  struck  by  another  vehicle.  It  will  be  shown  that  the  velocity  obstacle  sets  derived 
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in  Chapter  3  can  be  used  to  define  invariant  safe  states,  thereby  allowing  guaranteed 
infinite  horizon  collision  avoidance. 

2.3.1  Inevitable  Collision  States 

A  popular  variant  approach  to  guaranteed  safety  uses  Inevitable  Collision  States  [17, 
30].  Using  the  dynamics  of  the  vehicle  and  the  model  of  the  environment,  it  is  possible 
to  identify  vehicle  states  from  which  any  control  input  would  lead  to  a  collision.  If 
all  such  states  can  be  exhaustively  found,  then  the  host  vehicle  need  only  avoid  these 
states  to  guarantee  collision  avoidance. 

This  is  closely  related  to  the  use  of  invariant  safe  states:  maintaining  access  to  a 
safe  state  guarantees  safety;  alternatively,  never  entering  a  restricted  inevitable  colli¬ 
sion  state  achieves  the  same  goal.  Depending  on  the  scenario,  it  may  be  more  sensible 
to  satisfy  one  of  these  conditions  instead  of  the  other.  If  the  inevitable  collision  states 
are  easily  found,  it  is  straightforward  to  simply  check  that  the  commanded  input 
does  not  immediately  enter  such  a  state.  However,  in  the  case  of  multiple  obstacles, 
inevitable  collision  states  include  surrounded,  as  well  as  soon-to-be  surrounded,  con¬ 
figurations.  Solving  exactly  for  the  collisions  that  lead  to  unavoidable  collisions  is  a 
very  difficult  problem  whose  solution  may  be  more  naturally  expressed  in  terms  of 
invariant  safe  states. 
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Chapter  3 


Velocity  Obstacle  Set  Definition 


The  objective  of  this  work  is  to  find  conditions  that  guarantee  collision  avoidance, 
even  without  exact  knowledge  of  the  obstacles’  future  locations  as  functions  of  time. 
This  is  achieved  by  mapping  the  collision  regions  of  the  reachable  sets  of  the  obstacles 
(Section  2.1)  into  regions  in  velocity  space  and  using  the  velocity  obstacle  concept 
(Section  2.2)  to  produce  a  concise  representation  of  velocities  at  which  the  host  vehicle 
can  travel  without  ever  intersecting  the  collision  region  of  an  obstacle  for  any  time. 
By  keeping  clear  of  the  reachable  sets  of  each  obstacle,  the  host  vehicle  avoids  any 
possible  collisions  without  explicitly  anticipating  trajectories  for  the  obstacles. 

As  described  in  Section  1.2,  the  obstacles  are  assumed  to  follow  unicycle  dynamics: 
they  have  a  fixed  forward  speed  v  and  a  limited  angular  acceleration  of  \6\  <  uj,  this 
giving  them  a  minimum  turning  radius  of  p  —  In  addition  to  v  and  u,  the  collision 
radius  parameter  r  and  the  current  locations  and  headings  of  each  obstacle  are  used 
to  calculate  these  velocity  space  constraints. 

3.1  Definition 

Given  the  speed  and  turn-rate  constraint  of  any  unicycle  model  obstacle,  one  can 
compute  an  over-bound  SCR fit)  of  its  dynamically  feasible  collision  set  as  a  function 
of  time  (Section  2.1.2).  At  every  instant  in  time,  one  can  divide  SCRj(f)  by  the 
time  to  extract  a  corresponding  region  in  velocity  space,  the  simplified  velocity  region 
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(SVR),  expressed  as 


SVR  i(t)  =  SCy^  (3.1) 

for  any  t  >  0.  Any  point  mass  (that  starts  at  the  origin)  traveling  with  velocity  inside 
SVR i(t)  will  enter  the  collision  region,  whereas  a  point  mass  with  a  velocity  outside 
of  this  set  will  remain  clear  of  SCR i{t). 

The  boundary  Cj(f)  of  SVR *(f)  consists  of  the  segments 

Ql,2,3,4($V)  =  “‘Si, 2, 3, 4  (Q,t)  (3-2) 

Qs(t)  =  ]s5(t),  (3.3) 

using  Equation  2.7-Equation  2.14.  Any  input  outside  of  this  region  in  velocity  space 
will  return  a  linear,  single-speed  trajectory  that  will  be  safe  at  the  given  instant.  For 
safety  over  a  window  of  time,  one  needs  to  layer  all  the  instantaneous  constraints 
together.  The  union  of  these  constrained  regions  in  velocity  space  is  defined  as  the 
velocity  obstacle  set  (VOS)  (Figures  3-1  and  3-2),  which  thus  includes  all  of  the  single- 
velocity  trajectories  that  could  possibly  generate  a  collision  within  the  time  window. 
For  the  ith  obstacle, 

VOS;(f0, tf)  =  (JSVRi(t)  =  1J  S(  '/U/).  Vt  G  M/]-  (3.4) 

The  VOS  is  a  nonlinear  velocity  obstacle  computed  using  the  entire  reachability 
set  of  a  dynamic  object,  instead  of  just  a  single  estimated  trajectory  as  in  prior  work 
[25].  Velocities  outside  of  the  VOS  give  a  linear  trajectory  that  is  guaranteed  safe 
with  respect  to  all  dynamically  feasible  trajectories  of  the  obstacle  for  the  duration 
of  the  time  window. 

Equivalently,  the  VOS  can  be  interpreted  as  the  union  of  all  the  classic  nonlinear, 
single-trajectory  velocity  obstacles  (see  Figure  2-8  for  the  classic  nonlinear  velocity 
obstacle)  associated  with  each  of  the  dynamically  feasible  obstacle  trajectories,  de¬ 
fined  over  the  same  time  window  (see  Figures  3-3  to  3-5).  These  two  entities  would 


36 


VOS(to,tf) 


Figure  3-1:  Conceptual  sketch  of  example  velocity  obstacle  set  (region  in  velocity  space). 
VOS(to,tf )  is  the  union  of  SVR(t)  for  t  G  [to,tf].  The  boundary  of  each  SVR(t)  is  C(t). 
C(t )  is  composed  of  segments  Qi, 2, 3, 4(^1 1)  and  The  boundary  of  VOS  is  Ryos-  A 

point  P  on  the  boundary  has  an  associated  normal  vector  n. 


Candidate  boundary  points  of  VOS 


Figure  3-2:  Computed  example  velocity  obstacle  set  with  parameters  xq  =  4,  t/o  =  4, 
p  =  6.063,  r  =  1.5,  v  =  1.  C{to)  and  C(tf )  are  plotted  thicker  than  C{t)  for  intermediate 
times  (Each  C  is  plotted  as  its  components  <31,2,3,4,5).  These  ’’terminal  regions”  boundaries 
C(fo)  and  C(tf )  along  with  candidate  points  (in  blue  and  red)  found  on  <3i,2,3,4,5(t)  for 
intermediate  t  G  (to,t.f)  make  up  the  boundary  of  VOS(to,tf). 


be  exactly  equivalent,  as  they  both  return  the  set  of  velocities  that  may  put  the  robot 
within  contact  distance  of  an  obstacle  location  at  some  future  time.  The  only  dis¬ 
tinction  to  be  made  is  that,  as  the  VOS  is  defined  using  the  simplified  region  SCR, 
the  VOS  is  actually  a  superset  of  the  union  of  all  feasible  velocity  obstacles,  i.e., 


lfo°i  traj  {t0,  tf )  C  VOS{t0,  tf ) 


for  all  feasible  trajectories.  It  will  be  shown  in  section  3.2.4  that  the  two  are  in  fact 
very  nearly  the  same,  and  the  simplification  typically  does  not  affect  the  final  result 
(they  are  in  fact  identical  if  the  final  boundary  of  the  VOS  does  not  contain  elements 
of  Q5). 
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Velocity  Obstacles  for  a  selection  of  possible  obstacle  trajectories 


V 


Figure  3-3:  Velocity  obstacles  for  different  possible  trajectories  of  an  obstacle  with  .To  =  4, 
yo  =  (1,  p  =  6.063,  r  =  1.5,  v  =  1.  Jointly  avoiding  all  possible  velocity  obstacles  would 
guarantee  safety.  Exhaustively  simulating  all  these  trajectories  out  to  infinity  is  not  a  good 
solution. 

As  discussed  in  the  following  sections,  using  the  reachability  sets  as  a  function 
of  time  is  a  tractable  way  to  compute  the  VOS.  On  the  other  hand,  it  would  not 
be  computationally  feasible  to  arrive  at  the  same  theoretical  result  by  simulating  all 
possible  physical  trajectories  over  an  infinite  time  horizon,  computing  the  associated 
velocity  obstacles,  and  then  taking  the  union  of  the  results  (Figure  3-3).  Ultimately, 
the  desired  result  is  a  representation  of  the  boundary  of  the  VOS. 


3.2  Conditions  for  Boundary  Points  of  the  VOS 

This  section  derives  Algorithm  1  for  finding  the  boundary  iJyos^OU/)  of  the  region 
VOSj(£o,£/),  where  to  >  0  and  t/  <  oo  (See  Section  3.3.2  for  a  detailed  discussion 
of  these  limits).  All  candidate  points  from  the  curves  Ci(t)  whose  time  derivatives 
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VOS(t0,tf)  and  boundary  BVOS^tp 


Figure  3-4:  Velocity  Obstacle  Set  for  the  same  conditions.  This  set  is  computed  using 
reachability.  It  is  always  contains  the  region  in  Figure  3-3  as  a  subset;  in  this  case  they  are 
identical  if  the  region  in  Figure  3-3  were  exhaustively  defined. 

are  perpendicular  to  the  normal  vector  are  found.  These  points  define  the  points  of 
tangency  between  SVR *(£)  and  SVR i(t  +  6t)  (see  Figure  3-1).  The  set  of  all  points  sat¬ 
isfying  this  condition  contains  the  boundary  Ryos^^o,  tf),  as  summarized  in  Remark 
1.  A  detailed  explanation  follows. 

Every  point  P  on  -£>voSi(^o>  ^/)  must  be  a  boundary  point  on  at  least  one  of  the 
underlying  simplified  velocity  regions  SVR i(t)  for  some  t  e  (Figure  3-1).  This 

follows  trivially  from  the  fact  that  P  must  be  found  in  some  SVRj(f)  by  definition 
of  VOS*  in  Equation  3.4,  and  that  if  P  were  on  the  interior  instead  of  the  boundary 
Ci(t )  of  SVRj(t),  some  neighboring  point  of  P  would  lie  outside  of  -BvosJp- 

With  the  exception  of  corners,  every  point  P  on  the  curve  RvoSi(^o,  tf)  has  a  local 
outward  normal  vector  n\p.  See  Figure  3-7  for  an  example  of  a  corner.  Corners  in 
the  boundary  would  have  two  different  normals  approaching  from  either  side,  and 
the  normal  at  that  point  is  undefined.  These  intersections  between  local  boundary 
segments  are  dealt  with  at  the  last  step  in  this  algorithm,  but  the  following  conditions 
applied  locally  to  each  segment  and  its  normal  are  still  valid  as  necessary  conditions 
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Candidate  boundary  points  of  VOS 


i  Q 
Q 


1,2,5 


3,4,5 

. C(tf  =  inf) 

- candidates  on  Q 

.............  candidates  on  Q 

- candidates  on  Qr 


1,2 


3,4 


possible  paths  of  obs. 


Figure  3-5:  This  is  how  the  boundary  of  the  VOS  is  found.  Note  that  in  this  Figure  (as 
well  as  Figure  3-3),  the  physical  trajectory  is  just  a  reference  image. 

of  boundary  points. 

By  definition  of  a  boundary,  there  must  not  be  any  neighboring  points  inside 
VOS* (f0,  f/)  that  reach  farther  out  in  the  direction  of  n\p.  This  condition  implies  the 
standard  equation  for  the  normal  vector 

d 

n | p  ■  -g^By0Si\p(s)  =  0, 


where  s  is  any  variable  parameterizing  the  curve  -ByoSi-  -ByoSi(s  i  $s)  gives  relevant 
candidate  neighboring  points  of  P  within  the  set  VOS*  that  must  not  be  farther  out 
along  h\P.  Similarly,  since  P  is  found  in  some  SVR*(f)  which  is  in  VOS*,  one  can  also 
write  the  necessary  condition 


n\p  ■  jrCi\p(s)  =  0  (3.5) 

os 

for  any  variable  s  for  which  C*(s)  -  the  boundary  of  SVR*  -  is  continuously  defined 
such  that  Ci(s  ±  5s)  G  VOS*(to,t/). 
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Ct  is  composed  of  the  segments  Qi, 2, 3, 4(6*,  6)  and  Q^it)  from  Equation  3.2  -  Equa¬ 
tion  3.3,  and  the  parametric  expressions  for  these  segments  can  each  take  the  place 
of  Cj  in  Equation  3.5,  yielding 


d 

n\p  '  g-^Qi,j\p(s)  =  0, 


(3.6) 


where  i  indexes  the  dynamic  obstacle,  and  j  €  {1, ...  ,5}  as  piece-wise  segments  of 

Q. 

By  choosing  s  =  8  where  8  is  the  parameter  for  the  segments  <51,2,3,4(6*,  t),  Equa¬ 
tion  2.6  is  recovered  (scaled  by  a  factor  of  |) 

dS- 

n  ■  =  0  j  =  1, . . . ,  4,  (Equation  2.6) 

the  solution  of  which  is  Equation  2.5: 


h{6) 


sin  0 
cos  8 


(Equation  2.5) 


This  means  that,  if  P  is  found  on  some  segment  Qi, 2, 3, 4(8,  t),  then  the  normal  vector  n 
associated  with  By  os*  is  the  same  normal  vector  associated  with  SCR  *(f)  and  SVR  *(t). 
Similarly,  Q§(t)  could  be  generically  parameterized  by  arc- length  s,  recovering  the 
normal  vector 


n\p,Qs(t) 


(3.7) 


Intuitively,  these  results  indicate  that  the  normal  vectors  associated  with  the  over¬ 
all  boundary  Byos.(t0,tf)  at  any  point  P  must  match  with  the  normal  vectors  found 
locally  on  the  segments  Qj(t)  at  P  (see  Figure  3-1).  Otherwise,  a  point  on  Q3(t) 
would  lie  outside  the  boundary  Ryos,  (0, 6/). 

The  expression  for  n\p  can  then  be  substituted  into  the  necessary  condition  (Equa- 
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tion  3.6  with  the  time  parameter  t  substituted  for  s)  to  obtain 


and 


sin  9 

cos  9 


lQ  I 


(9,  t)  —  0;  je  4} 


(3.8) 


0 

1 


•  Qj.Qi,5\p(t)  —  o 


(3.9) 


as  necessary  conditions  for  a  point  P  to  be  on  the  boundary  -BvoSi^o,  tf),  as  long  as 
Qij(9,t±6t)  G  VOS i(to,tf).  This  latter  statement  is  true  for  all  t  in  the  open  interval 
( t0,tf ).  (At  the  endpoints  t0  and  tf  themselves,  these  conditions  need  not  hold;  just 
like  how  the  global  minimum  of  a  function  on  a  closed  interval  can  be  found  on  the 
end  of  the  interval  without  the  needing  the  derivative  to  be  0  there.)  This  condition 
drives  how  the  boundary  is  found. 

Remark  1.  Any  point  P  in  Byos.fto,  tf)  must  either  satisfy  Equation  3.8  or  Equa¬ 
tion  3.9,  or  lie  on  the  curves  Ci(t0)  or  Ci(tf).  Graphically,  Equation  3.8  and  Equa¬ 
tion  3.9  describe  points  that  are  local  tangent  points  between  SVRft )  and  SVRi(t  +  5t ) 
(Figure  3-1).  Finding  all  such  points  results  in  a  larger  set  that  contains  all  possible 
points  in  Byosjloi  tf)-  This  set  also  includes  local  extrema  that  are  not  necessarily 
global  boundary  points;  see  Figure  3-6.  Algorithm  1  is  the  result  of  these  conditions. 


3.2.1  Solving  the  Necessary  Conditions 

This  section  show  in  detail  how  Equation  3.8  and  Equation  3.9  are  solved.  Without 
loss  of  generality,  assume  the  host  robot  is  located  at  the  origin,  and  the  obstacle  is 
located  at  (xo,yo),  pointing  along  the  positive  y  axis  (so  that  the  heading  is  zero). 
A  different  initial  condition  can  be  first  rotated  to  match  this  heading,  and  the  cor¬ 
responding  (x0,y0)  is  easily  computed,  and  the  final  result  is  rotated  back  to  the 
original  frame  (lines  1  and  20  in  Algorithm  1). 
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Algorithm  1:  Solving  for  the  boundary  of  VOS  (See  figure  3-6) 

Input:  obstacle*  :  {x0,y0,90,v,  p},t0,tf 
Output:  ByoSi 

1  rotate  (. x0,y0 )  by  —  90  (vehicle  heading); 

2  {candidate  segments}  G-  0; 

//  add  C(t o)  and  C(tf)  to  consideration 

3  for  t  G  {t0,  tf}  do 

4  find  C(t)  using  (3. 2), (3. 3); 

5  add  C{t)  to  {candidate  segments}  ; 

//  add  candidate  points  on  Qi,2 

6  {critical  angles}  =  solve  conditions  in  (3. 13), (3. 12); 

7  for  9*  G  {critical  angles}  do 

s  ti  =  max(^,t0); 

//  points  make  linear  segment  between  <5i,2(0*,ti)  and  Qi^(9* ,tf) 

9  find  segments  using  (3.10); 

10  add  segment  to  {candidate  segments}; 

//  add  candidate  points  on 

n  {local  segments}  0; 

12  for  t  G  [to, min(-, tf)]  do  //  discretize  t 

13  {points}  =  solve  conditions  in  (3. 17), (3. 16)  using  Equation  3.24; 

14  append  {points}  onto  {local  segments}; 

15  add  {local  segments}  to  {candidate  segments}  ; 

//  add  candidate  points  on  Q5 

16  th  =  solve  condition  in  (3.19); 

//  endpoints  of  Q5(th)  =  Q{2,i}or{4,3}(±7T,  th) 
n  find  Q5(th )  using  (3. 10), (3. 15); 
is  add  Qsith)  to  {candidate  segments}; 

//  candidate  segments  now  includes  all  possible  boundary  points 

19  boundary  =  combine  {candidate  segments}; 

20  rotate  boundary  by  90 ; 

21  return  boundary; 


Candidate  boundary  points  of  VOS 


vx  (m's) 

Figure  3-6:  Example  candidate  points  in  Byos(to,tf),  using  parameters  xq  =  4,  yo  =  —4, 
p  =  6.063,  r  =  1.5,  v  =  1.  Various  instances  of  C(t)  are  outlined  in  green  and  magenta. 
Candidate  boundary  points  drawn  in  red,  blue,  and  black.  The  physical  obstacle  is  sketched 
for  reference,  but  note  that  the  rest  of  the  plot  is  in  velocity  space,  and  the  physical  obstacle 
cannot  actually  be  expressed  in  these  coordinates  and  needs  to  be  on  separate  axes,  as  done 
technically  correctly  in  Figures  2-5,  2-6,  and  2-7. 


i)  Points  on  Qip(9,t): 

This  corresponds  to  lines  6-9  in  Algorithm  1.  First  consider  points  on  the  segments 
Q i,2(0)  £)  that  may  satisfy  Equation  3.8.  From  Equation  3.2  and  Equation  2.8, 


Q2(0,  t)  —  — 


x0  +  p(  1  —  cos  9)  +  {vt  —  p6  +  r)  sin  6 
yo  +  p  sin  6  +  (vt  —  p6  +  r)  cos  6 


(3.10) 


The  domain  for  Q2(6,t)  is 


Q2(9,t)  :  t  G  [t0,tf],  9  G  [0,min(u;t,7r)], 


(3.11) 
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VOS  (fry,  tf)  and  boundary 


Figure  3-7:  Final  boundary  of  VOS  from  example  in  figure  3-6.  The  same  parameters  of 
xo  =  4,  j/o  =  —4,  p  =  6.063,  r  =  1.5,  v  =  1  are  used.  C(to  =  e)  and  C(tj  — >  oo)  are  outlined 
in  red  and  magenta.  By  os  drawn  in  blue.  The  maximum  host  vehicle  velocity  drawn  in 
black;  C(to  =  e)  lies  outside  of  this  circle. 


which  comes  directly  from  Equation  2.10.  Taking  the  derivative  with  respect  to  time 
and  substituting  into  Equation  3.8  yields 


dQ2(9,t) 

dt 


_1 

72 


(x0  +  p)  +  p  cos  6  +  (r  —  p6)  sin  9 
y0  —  p  sin  6  +  (r  —  pO)  cos  9 


0 

0 


sin# 
cos  9 


((to  +  P )  sin  9  +  y0  cos  9  +  r 


p»). 


(3.12) 


For  any  t  and  9  in  the  specified  domain  that  satisfies  Equation  3.12,  Q2(9,t)  is  a 
potential  boundary  point  in  5VOs(to,  t/). 
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Identical  steps  lead  to  the  necessary  condition 


(( xo  -  P )  sin6>  +  |/o  cos  9  +  r  +  p9)  =  0  (3.13) 

tz 

for  points  of  Qi(9,t),  defined  in  the  domain 

Qi(0,t)  :  t  G  [to,tf\,  9  G  [max(—  ut,  —  7r),  0]. 

For  any  combination  of  t  and  9  within  the  domain  of  Qi  that  satishes  Equation  3.13, 
Qi(9,t)  is  a  potential  boundary  point  in  Byos(to,tf). 

Note  that,  excluding  t  —  oo  and  assuming  t  ^  0,  Equation  3.12  and  Equation  3.13 
can  be  reduced  to  functions  of  9  alone.  Each  has  at  most  two  values  of  9*  that  satisfy 
the  equation  within  its  domain,  and  these  values  are  easily  found  numerically.  Candi¬ 
date  boundary  points  are  given  by  Qj(9*,t )  for  t  G  [ti,t/],  where  t\  =  rna x(9*/w,to), 
since  for  any  t  <  e*/u,  9 *  would  not  be  in  be  domain  of  9  (see  Equation  3.11). 

For  a  solved  value  of  9*,  the  locus  of  all  candidate  boundary  points  is  a  linear 
segment  between  Qj(9*,ti)  and  Qj(9*,tf)  for  j  =  {1,2}  (red  lines  in  Figure  3-6). 
This  is  because  Qj(9*,t )  Equation  3.10  is  a  linear  function  of  },  which  is  well  defined 
for  t  ^  0. 

Physically,  if  the  dynamic  obstacle  were  turn  to  at  maximum  rate  to  to  9*  and 
then  continue  driving  directly  in  this  direction,  in  order  to  avoid  collision,  the  host 
robot  would  need  to  choose  a  path  that  is  deflected  away  from  the  obstacle  with  the 
widest  clearance,  compared  to  what  it  would  have  to  done  to  avoid  any  other  feasible 
obstacle  trajectory. 

ii)  Points  on  Q3,4(9,t):  This  corresponds  to  lines  11-15  in  Algorithm  1. 

Next,  all  possible  boundary  points  on  Q 3)4($,  t)  are  found.  Like  segments  t ) 

(Equation  2.11-Equation  2.12),  the  domains  of  segments  Qz^{9,t)  are 

te[t0,tf],  9  G  [— 7T ,  —ut\ , 
t  G  [to,  tf],  9  G  [ft,  cot]. 


47 


(3.14) 


Due  to  the  limits  on  the  domain  of  9,  when  solving  for  Byos(t0,tf),  one  only  needs 
to  consider  points  in  Q3.4  from  times  in  the  range  t  G  [to,  min(7r/«',  £/)].  As  seen  in 
Figure  3-6,  as  t  increases,  Q 3,4  eventually  disappear. 

From  Equation  3.2  and  Equation  2.12, 


Q*i(9,t) 

dQi 

~dt 


1 

t 

-1 


Xo  +  p(l  —  cos(t ut))  +  r  sin# 
yo  +  p  sin(u;t)  +  r  cos  9 

(x0  +  p)  —  pcos(ut)  —  put  sin(u;t)  +  r  sin  6 
y0  +  psm(ut)  —  put  cos  (ut)  +  r  cos  9 


(3.15) 


Given  any  value  of  t  from  within  the  domain,  the  necessary  condition  from  Equa¬ 
tion  3.8  requires 


jtQi(  o,t) 


sin  9 
cos  9 


=  0, 


(3.16) 


which  can  be  solved  numerically  for  9.  Alternatively,  see  Section  3.2.3  for  an  equiva¬ 
lent  analytic  geometric  solution  to  these  conditions.  For  a  given  t,  Equation  3.16  has 
between  zero  and  two  valid  solutions  for  9  G  [0,  2n)  (this  is  clear  from  the  geomet¬ 
ric  interpretation).  Any  of  these  solutions  that  fall  within  the  domain  9  G  [ut,  7r]  is 
considered  a  valid  candidate  boundary  point  in  Byos(t0,tf). 

Similarly,  for  Q3(9,t),  using  Equation  2.11 


9Q  3 
~dt 


1 

¥ 


(xo  +  p)  +  p  cos  (ut)  +  put  sin(u;f )  +  r  sin  9 
Do  +  p  sin  (ut)  —  put  cos  {ut)  +  r  cos  9 


Given  a  value  of  t,  the  necessary  condition 


!«><*•*> 


sin  9 
cos  9 


0 


(3.17) 


can  be  solved  numerically  for  9,  and  solutions  that  are  in  the  domain  9  G  [— n,  ut]  are 
considered  valid  candidate  boundary  points. 
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While  the  sets  of  all  possible  boundary  points  from  segments  Q i  and  Q 2  define 
linear  segments  given  by  just  their  endpoints,  the  set  of  candidate  points  (The  dotted 
blue  segments  in  Figure  3-7)  found  in  and  Q 4  in  general  cannot  be  represented 
analytically  in  a  more  compact  form.  Note  that  for  Q  lj2,  one  could  first  solve  inde¬ 
pendently  for  9 *,  while  here,  the  variables  cannot  be  neatly  decoupled.  Instead,  one 
must  discretize  the  finite  interval  [to,  min(7r/^,  t/)]  at  some  resolution,  and  at  every 
discrete  value  of  t,  points  satisfying  Equation  3.16  or  Equation  3.17  are  found.  Solu¬ 
tions  points  from  sequential  instances  in  time  are  matched  together  to  form  candidate 
boundary  segments.  The  resolution  of  these  segments  depend  on  how  finely  the  time 
interval  is  discretized.  Note  that  the  duration  of  this  interval  is  upper-bounded  by 
n/u,  the  time  it  takes  the  obstacle  to  turn  around,  thus  capping  the  computational 
demands  of  this  process. 

iii)  Points  on  Qs(t):  This  corresponds  to  lines  16  -  18  in  Algorithm  1. 

Finally,  points  on  Qs(t)  for  any  t  G  [f0,t/]  that  may  be  part  of  ByOS(t0,tf )  need 
to  be  found.  Again,  the  definitive  necessary  condition  is 


-  9Q  5  n 

"'^r  =  0' 


(3.18) 


where  h  —  0  —1  (from  Equation  3.7)  and  Qs(t)  is  the  horizontal  line  segment 
joining  Qi(— 7T,  t)  and  t ),  or  joining  Qs(— 7T,  t)  and  QA71,  t ),  depending  on  which 

set  of  domains  of  6  contain  ±7r  at  the  given  t.  Since  Q§(t)  is  a  straight,  horizontal 
segment,  this  condition  holds  true  when  the  two  endpoints  satisfy  their  respective 

r  1  t  r  -|  t 

conditions  of  being  normal  to  n\p(0)  —  sin  9  cos  9  —  0  —  1 

When  ut  >  1 r,  Qs(t)  joins  Q 2,i(±7t, f),  and  Equation  3.18  is  satisfied  if  ±7r  are 
solutions  to  Equation  3.12  and  Equation  3.13.  When  this  is  the  case,  it  is  easy  to 
show  that  all  the  points  in  Qs(t)  f°r  t  >  n/w  line  up  with  the  linear  segment  between 
Q2,i(Q*,ti)  and  Q2,i(9*,tf)  described  earlier  (where  9*  =  ±7r).  These  points  do  not 
need  to  be  accounted  for  a  second  time. 
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When  ut  <  7 r,  Q^{t)  joins  Q^3(±tt,  t),  and  Equation  3.18  yields 

y0  +  psin(ujth )  —  pcoth  cos (coth)  —  r  —  0  (3.19) 

This  can  be  numerically  solved  for  th-  Using  Q3(— ir,  th)  and  Q^tt,  th),  Qbith)  (the 
black  line  in  Figure  3-6)  is  then  explicitly  found  and  added  to  the  set  of  candidate 
boundary  points. 

See  section  3.2.4  for  discussion  about  implications  of  using  Q 5  to  form  the  simpli¬ 
fied  velocity  region  in  place  of  the  exact  boundary. 

iv)  Combining  the  Results:  (line  19  in  Algorithm  1;  see  Figures  3-6  and  3-7) 

As  described  in  Remark  1,  considered  together  with  the  curves  C(t0 )  and  C(t/), 
the  solutions  to  Equation  3.12,  Equation  3.13,  Equation  3.16,  Equation  3.17  and 
Equation  3.19  in  their  specified  domains  exhaustively  describe  all  points  that  could 
lie  on  the  boundary  -BvoSi(OV/)-  These  pieces  of  the  boundary  are  found  as  seg¬ 
ments  and  points,  and  they  need  to  be  sorted  into  a  continuous  curve.  After  all  the 
components  are  found,  the  arguments  t  and  6  for  each  piece  are  used  to  identify  its 
neighboring  components,  thus  allowing  the  segments  and  points  to  be  correctly  sorted 
into  a  single  curve. 

This  loop  may  intersect  itself,  in  which  case  the  segments  on  the  interior  are 
not  parts  of  the  actual  boundary  BVOS.  This  phenomenon  is  perfectly  consistent, 
since  the  conditions  that  were  solved  for  were  simply  necessary,  but  not  sufficient , 
conditions  for  points  on  BVOS.  Intersections  on  this  loop  of  candidate  boundary 
points  form  corners  in  the  final  boundary  and  are  easily  detected  by  checking  each 
segment  in  the  loop  in  order.  It  is  easy  to  identify  the  segments  that  are  on  the 
interior,  and  these  segments  are  truncated  out  of  the  solution.  The  remainder  of  the 
continuous  curve  is  the  perimeter  BVOS. 

3.2.2  Computational  Complexity 

For  each  obstacle,  Equation  3.12,  Equation  3.13,  and  Equation  3.19  each  need  to  be 
solved  only  once,  as  a  function  of  the  single  variable  6.  Equation  3.16  and  Equa- 
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tion  3.17  need  to  be  repeatedly  solved  as  functions  of  9  at  discretized  instances  of  t, 
but  the  range  of  t  is  bounded  as  t  G  [to,  min^/w,  tf)].  For  each  t,  the  process  out¬ 
lined  in  3.2.3  gives  a  straight-forward  and  efficient  formula  for  finding  the  solutions 
P(9,t).  Ci(t0 )  and  Ci(tf )  are  directly  found  using  Equation  3.10  and  Equation  3.15 
(plus  similar  equations  for  Qi^).  All  of  these  computations  are  very  light  and  can 
easily  be  done  on-line.  Calculating  and  representing  the  conditions  for  each  obstacle 
that  guarantee  collision  avoidance  is  thus  a  simple  process  that  can  be  implemented 
in  real-time  at  each  planning  iteration.  How  a  planner  then  uses  and  checks  these 
constraints  is  a  separate  problem. 


3.2.3  Geometrical  Interpretation 

ii)  Points  on  Q3t±{9,t)m.  Consider  a  point  (in  velocity  space)  P  on  the  curve  Q% 

p{t,B)  =  Q3{t,e ) 


for  some  t  and  6  such  that  P  is  on  the  boundary  Hyos-  The  solution  for  P  on 
Q 4  is  derived  the  exact  same  way  after  flipping  two  signs  in  the  x  coordinate  (See 
Equation  3.15).  By  definition  Equation  3.2, 

Pit,  o)  =  Q3M)  =  ls3M), 

where  S3(t,  6)  is  a  point  on  the  circular  arc  of  radius  r  centered  on  the  bottom  corner 
of  the  reachability  set  (See  Figure  2-1  and  Equation  2.11): 

o  u  m  X°  +  P(l  ~  cosM))  ,  sinW 
S3{t,0)=  +  r 

Do  +  psin{ujt)  cos(0) 

Thus,  the  partial  derivative  dP^f'>  can  be  re-written  as 


dP(t,  9)  _  d 
dt  dt 


(1  x0  +  p(l  -  cos(ut)) 
y0  +  p  sin  (ut) 


d_ 

Ft 
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dq  d 

m  +  m 


c  +  b, 


where  q,  c  and  b  are  defined  as 


fit) 


c(t) 

bit ,  9) 


x0  +  p{  1  —  cos(u;i)) 
Vo  +  p  sin  (cat) 


dq  1 

( 

xo  +  P 

—  COS  iut) 

dt  t 2 

\ 

yo 

+  P 

sin(o;t) 

8  (1,  , 

\  r 

\\m\  = 

—  (  —  (r  n 
dt  \tK 

p) 

=  -ji  n\ p, 

+  put 

r 

¥ 


sin(u;t) 
cos  (tat) 


(3.20) 

(3.21) 

(3.22) 


(see  Figures  3-8  and  3-9  for  geometric  representation  of  these  vectors).  Note  that  the 
directionality  of  b  depends  on  6  (through  n\p),  whereas  the  magnitude  is  purely  a 
function  of  t.  As  derived  earlier  (Equation  3.8),  the  necessary  condition  for  P  to  be 
a  boundary  point  is 


dP{t,  6) 
dt 


sin(d) 

cos(0) 


0, 


i.e.,  the  vector  dPQt'e'>  must  be  perpendicular  to  the  normal  vector  h\p,  which  is  parallel 
to  b: 


d Pit,  6) 
~~dt 


c  +  b  _L  flip  ||  b. 


Hence,  the  vectors  9Pq1:,9'>  ,  c,  b  form  a  right  triangle  with  hypotenuse  c  (See  Figure 
3-8),  and  an  angle  a  between  legs  c  and  b  given  by 


a(t) 


(3.23) 


Thus,  given  t,  it  is  computationally  trivial  to  evaluate  q,  c,  b,  and  a  using  Equa¬ 
tion  3.20  to  Equation  3.23.  With  q,  c,  and  a  (and  the  parameter  r),  it  is  simple  to 
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Figure  3-8:  How  to  find  candidate  points  P  on  Q 3,4  geometrically.  Given  t.  evaluate  q,  ||6||, 
and  c;  solve  for  a;  then  reconstruct  P  (and  second  solution  P2)  using  q  and  a.  See  Figure 
3-9  to  see  how  q  fits  into  the  overall  picture. 


solve  for  point  P  using 


P 

P 


arctan 


cos(/3) 
sin  (p) 


(3.24) 


This  yields  an  analytic  process  for  finding  points  P(t,9)  =  Qzit^O)  that  satisfy  the 
conditions  in  Equation  3.16  and  Equation  3.17,  such  that  they  are  potentially  on  the 
boundary  BVOS.  As  described  in  Section  3.2.1,  the  time  interval  [t0,  (j]  needs  to  be 
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Candidate  boundary  points  of  VOS 
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- Q1  2  5 - Q3  4  5  .  q  —  candidates  on  Q3  4 


Figure  3-9:  Candidate  boundary  points  on  Qs^.  See  Figure  3-10  for  context  of  this  detail. 
8  instances  of  SVR(t)  are  plotted  for  times  between  to  =  3,  tf  =  20,  with  the  first  and  last 
instances  thickened.  The  boundary  of  each  SVR  consists  of  Q12  in  magenta,  and  in 
green,  with  Q 3  on  the  left  and  Q4  on  the  right.  The  straight  horizontal  green  segment  is  Q 5. 
Note  that  Q 3,4  are  circular  arcs  centered  on  q.  The  candidate  points  P  found  are  plotted 
in  blue.  The  discretization  of  time  to  find  these  points  P  is  finer  than  the  discretization 
for  the  SVRs  shown.  q(t)  is  plotted  as  a  red  dot  for  each  SVR.  In  this  particular  example, 
note  how  there  are  valid  solutions  on  Q3  up  to  roughly  t  =  10  (halfway  between  to  and  tf), 
and  there  are  valid  solutions  on  up  to  roughly  t  =  5.  The  process  outlined  here  finds  2 
points  at  each  time  instance  ( P  and  P2  in  Figure  3-8),  but  in  this  example,  one  of  the  two 
at  every  instance  lies  outside  the  domain  of  6  for  (^3,4  and  is  discarded  (not  plotted).  These 
discarded  points  would  have  been  found  on  extensions  to  the  circular  arcs  that  would 
be  on  the  interior  of  each  SVR(t),  and  hence  not  relevant  as  candidate  boundary  points. 
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Candidate  boundary  points  ol'  VOS 


Figure  3-10:  Candidate  boundary  points  for  xq  =  —4,  y$  =  4,  p  =  6.063,  r  =  1.5,  v  =  1,  the 
same  situation  as  the  detail  shown  in  figure  3-9.  Note  that  in  this  case  all  candidate  points 
are  in  fact  on  the  boundary. 

discretized,  and  candidate  points  P(t)  can  be  found  using  Equation  3.24  for  each 
discrete  value  of  t. 

Note  that  in  Equation  3.23,  if  ||6(f)||  <  ||c(f)||  there  are  two  values  of  a  and  hence 
distinct  two  candidate  points  P(f);  if  =  ||&(t)||  =  ||c(f)||  there  is  one  solution  for  P(f); 
and  if  \\b(t)\\  >  ||c(f)||  there  are  no  valid  candidates.  The  corresponding  argument  6 
of  each  point  P  obtained  through  Equation  3.24  still  needs  to  lie  within  the  domain 
described  in  Equation  3.14  to  be  considered  a  valid  candidate  point  (See  Figure  3-9). 

3.2.4  Effect  of  simplifying  collision  region 

If  the  VOS  were  defined  as  exactly  the  velocities  that  could  contact  the  reachable 
set  of  the  obstacle  at  some  time  (using  the  collision  region  CR(f)  instead  of  the 
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simplified  collision  region  SCR(t)),  any  points  added  from  Q$(t)  for  some  t  would  not 
be  boundary  points  of  the  velocity  obstacle  set.  Instead,  these  candidate  boundary 
points  from  replace  boundary  segments  that  lie  in  the  interior  of  the  simplified 
VOS,  which  would  take  a  lot  more  effort  to  compute  (see  Figures  2-2  to  2-4). 

To  find  these  candidate  points  on  the  exact  boundary,  first  the  limits  on  the 
domains  of  A1>2  and  B  1}2  would  have  be  computed  for  \9\  >  i r  (see  Section  3.2.1  in 
[4]).  Then,  to  find  possible  boundary  points  within  these  domains,  the  computational 
burden  would  be  very  similar  to  that  of  finding  points  on  Q  1,2,3,41  the  major  hurdles  lie 
in  re-deriving  all  the  equations  and  conditions  using  Equation  2.3  and  Equation  2.4, 
and,  in  the  implementation  itself,  of  combining  points  and  segments  together  into 
a  continuous  boundary.  Tracing  the  exact  curve  of  the  reachable  set  would  imply 
that  9  no  longer  uniquely  defines  a  point  on  C(t)  for  a  given  t  (SCR(t)  is  convex, 
whereas  CR (t)  is  not;  see  Figure  2-2),  and  additional  rules  would  need  to  be  built 
into  the  function  that  sorts  candidate  points  and  segments  into  an  ordering  to  form 
a  continuous  loop. 

Note  that,  especially  in  cases  for  which  tf  —  oo,  very  rarely  (empirically,  less  than 
1%  of  the  time)  do  any  points  from  Q$  make  it  onto  the  final  boundary  Ryos-  As  is 
the  case  in  Figures  3-5  and  3-6,  the  candidate  points  on  Q5  tend  to  end  up  within 
other  candidate  boundaries  formed  by  points  on  Q  1,2, 3, 4,  and  are  thus  excluded  from 
By  os-  When  no  points  from  Q  5  are  found  on  the  final  boundary,  none  of  the  points 
of  the  original  collision  region  omitted  in  the  simplification  could  have  been  on  the 
boundary  either,  so  the  VOS  found  using  the  simplification  is  then  in  fact  identical 
to  what  would  have  been  found  using  the  exact  collision  regions,  i.e., 


VOS  (t0,tf)  =  LJvo|  traj 


While  these  points  from  Q§  seldom  affect  the  final  VOS,  it  is  nonetheless  crucial  for 
the  sake  of  completeness  that  the  algorithm  finds  these  candidate  points  before  dis¬ 
carding  them,  for  otherwise  the  proof  of  collision  avoidance  would  be  invalidated.  The 
simplification  made  by  using  Q5  is  well  suited  for  serving  this  role  as  an  intermediate 
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step  in  the  proof. 

When  the  VOS  is  calculated  with  a  finite  time  horizon,  it  is  fairly  common  for 
the  segment  Qsitf)  to  appear  on  -Bvos(h), tf).  In  general,  these  points  do  not  satisfy 
Equation  3.9,  but  all  points  on  C(tf)  are  also  considered  candidate  boundary  points, 
and  Q5(tf)  is  a  component  of  C(tf )  for  any  tf  <  oo  (see  Figure  3-11).  When  this  is 
the  case,  a  small  part  of  the  boundary  of  the  exact  collision  region  lies  on  the  interior 
of  the  VOS  as  defined,  so 


VOS(t0,tf)  D  |JV0|  traj 

such  that  the  VOS  as  dehned  forms  a  (very  slightly)  conservative  bound  on  the  single- 
velocity  trajectories  that  are  safe  up  to  time  tf. 

3.3  Upper  and  Lower  limits  of  time  window 

3.3.1  Lower  limit  of  time  window 

Just  as  in  the  original  velocity  obstacle  formulation  [5],  as  t0  — >  0+,  the  velocity 
obstacle  set  VOS(to,tf)  extends  out  to  infinity  in  the  direction  of  the  current  location 
of  the  physical  obstacle  (reviewed  in  Section  2.2).  This  comes  from  increasingly  higher 
speeds  required  to  collide  with  the  obstacle  on  decreasing  time  scales,  and  beyond  a 
certain  point,  these  velocities  are  irrelevant  since  they  are  beyond  the  capacities  of 
the  host  vehicle;  these  potential  collisions  as  t  — >  0  are  not  physically  possible.  The 
velocity  space  constraint  SVR (t  =  0)  is  undefined,  since  contact  between  the  vehicles 
at  this  time  depends  only  on  the  initial  conditions,  not  any  input  velocity. 

As  t  =  0  cannot  be  used,  a  practical  approach  is  to  set  t0  at  some  e  >  0  such  that 
SVR(to  =  e)  lies  beyond  input  velocities  on  interest.  One  method  is  to  set  the  entire 
region  SVR(e)  beyond  some  maximum  speed  i’max  that  the  host  vehicle  can  achieve 
(In  Figure  3-12,  e  is  chosen  such  that  the  red  boundary  of  SVR(f0  =  e)  is  outside  of 
the  circle  of  the  host  robot’s  maximum  speed). 

This  is  more  easily  done  by  considering  the  dynamic  obstacle  without  turn-rate 
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Candidate  boundary  points  of  VOS 


VOStfo,  f/i  and  hcmtci.'.ry 


v  I’rru'sl 

K  ‘ 


Figure  3-11:  Here,  xq  =  4,  yo  =  —4,  p  =  6.063,  r  =  1.5,  v  =  1  as  in  figure  3-6,  but 
tj  =  Msec.  The  simplifying  segment  Qs(tf)  is  part  of  C(tf ),  hence  the  simplification  does 
indeed  affect  the  final  boundary.  Without  the  simplification,  the  less  conservative  final 
boundary  would  include  a  curved  segment  that  lies  on  the  interior  of  the  straight  segment. 
This  curve  cannot  be  solved  for  using  the  methods  presented  in  this  thesis. 


VOS  (fry,  tf)  and  boundary 


Figure  3-12:  VOS(to,t/)  computed  for  obstacle  parameters  xq  =  4,  yo  =  —4,  p  =  6.063, 
r  =  1.5,  v  =  1.  Using  the  process  detailed  in  Section  3.3.1,  to  =  1.188  was  chosen  such  that 
SVR(fo)  lies  outside  of  the  set  of  feasible  host  vehicle  velocities. 


constraints  and  with  maximum  speed  v.  The  collision  region  (the  reachable  set  of 

the  obstacle,  grown  by  the  collision  radius  r)  associated  with  a  vehicle  subject  to 

fewer  dynamic  constraints  contains  the  collision  region  of  the  more  limited  vehicle, 

since  the  reachable  set  of  the  latter  is  a  subset  of  the  reachable  set  of  the  former. 

Therefore,  if  the  collision  region  at  time  e  of  a  vehicle  moving  with  arbitrary  speeds 

<  v  lies  entirely  outside  of  the  circle  of  radius  vrnax ,  the  region  SVR(e)  for  a  vehicle 

with  a  limited  turn  rate  and  constant  forward  speed  v  that  starts  at  the  same  location 
r  i  T 


Xo  Vo 


also  lies  outside  of  this  circle. 


The  reachable  set  at  any  time  £  of  a  vehicle  with  only  a  maximum  speed  constraint 


starting  at 


1  T 


Xo  Vo 


at  t  =  0  is  simply  a  disk  of  radius  vt  centered  on 


1  T 


xo  yo 


see 


Figure  3-13).  The  collision  region  around  this  reachable  set  has  radius  vt  +  r,  so  in 
velocity  space,  the  set  of  velocities  that  would  enter  this  collision  region  at  time  t  is 
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Figure  3-13:  Reachable  set,  collision  region,  and  collision  velocities  of  a  moving  obstacle 
with  no  turn-rate  constraints  and  a  maximum  speed  of  v.  If  this  region  in  velocity  space  lies 
outside  of  the  circle  of  radius  vmax  centered  on  the  origin,  then  SVR(f)  for  the  dynamically 
constrained  obstacle  also  lies  outside  of  the  circle. 
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T 


a  disk  of  radius  v  +  7  centered  on  \ 


x0  Vo 


.  Thus,  the  boundary  of  the  collision 


region  in  velocity  space  is  given  by  the  parametric  curve 


1 

t 


x0 

yo 


+  (r  +  vt) 


COS  0 

sin  0 


for  0  G  [0,27t).  Therefore,  to  choose  t0  =  e  such  that  this  disk  is  outside  of  the  host 
vehicle’s  feasible  velocities,  solve  for  e  such  that 


1 

x0 

e 

Vo 

(r  +  ve) 


COS  0 

sin  0 


>  vmax  V0  G  [0,2vr], 


This  yields  the  formula 

e  <  Vxl  +  y’o-  r 

Vmax  +  V 


3.3.2  Upper  limit  of  time  window 

For  an  obstacle  moving  along  a  known,  linear  trajectory,  the  velocity  obstacle  cone 
converges  to  an  apex  at  vobs  as  t  — >  00  [5].  For  the  velocity  obstacle  set  described 
here,  SVR(t  — >  00)  goes  to  a  circular  disk  of  radius  v  centered  on  the  origin.  This 
can  be  easily  derived  by  taking  the  limit  of  Equation  3.10  (noting  that  as  t  — >  00, 
C(t )  =  Q i)2(t,  0)  since  the  domain  of  Q3)4  disappears  at  t  =  7r/o,): 

x0  +  p(  1  —  cos  6)  +  (vt  —  p6  +  r)  sin  9 
yo  +  p  sin  9  +  (vt  —  p9  +  r)  cos  9 

v  sin  9 
v  cos  9 

Intuitively,  lmp.^  SVR(t)  is  this  circle  of  radius  v  because,  if  the  host  vehicle 
were  to  drive  along  any  straight  trajectory  in  any  direction  with  a  constant  speed  less 
than  v,  the  obstacle  could  always  eventually  catch  up  and  collide.  This  implies  that, 


lim  Q2(9,t)  =  lim  - 

t—> OO  t— >OQ  t 
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to  guarantee  avoidance  of  the  dynamic  obstacle  on  the  infinite  horizon  without  the 
host  robot  changing  its  velocity,  the  host  robot  must  be  able  to  drive  faster  than  the 
obstacle. 

Choosing  a  velocity  that  is  not  inside  of  the  infinite  horizon  velocity  obstacle  set 
VOS (to,tf  =  oo)  provides  the  host  vehicle  with  a  single-velocity  trajectory  that  will 
never  enter  the  collision  region  of  the  moving  obstacle.  The  fact  that  VOS(to,tf  =  oo) 
is  well  defined  is  a  very  powerful  result  for  infinite  horizon  safety,  which  is  discussed 
rigorously  in  Chapter  4. 
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Chapter  4 


Safety  Guarantees 

4.1  Introduction 

As  explained  in  the  beginning  of  Section  3.1,  any  velocity  outside  of  VOS i(t0,tf)  for  a 
given  obstacle  i  can  be  used  to  generate  a  single- velocity  trajectory  that  is  guaranteed 
to  avoid  any  dynamically  feasible  path  of  that  obstacle,  for  the  duration  of  the  time 
window.  For  multiple  obstacles,  the  constraints  are  simply  combined;  a  velocity  that 
is  outside  of  the  VOSs  of  each  of  the  obstacles  is  guaranteed  safe  for  the  duration. 
The  only  remaining  problem  is  that,  if  tf  is  set  to  some  finite  value  r,  there  is  no 
guarantee  that  this  safety  can  be  propagated.  For  any  t'  <  r,  the  host  vehicle  would 
avoid  all  collisions,  but  nothing  can  be  said  about  the  continued  existence  of  safe 
trajectories  of  duration  t  —  t!  or  longer.  This  is  the  major  problem  encountered  by 
other  collision  avoidance  methods  that  use  finite  time  horizons  r  [9]. 

However,  in  this  formulation,  it  is  perfectly  tractable  (Section  3.3.2)  and  reason¬ 
able  (because  there  is  no  prediction  of  obstacle  behavior  involved)  to  set  tf  at  oo, 
and  this  allows  the  use  of  a  slightly  modified  version  of  the  concept  of  invariant  safe 
states  (Section  2.3).  As  described  in  [2],  these  are  host  vehicle  states  that 

•  and  can  always  be  propagated  back  to  themselves. 

•  meet  all  imposed  safety  conditions, 

In  [2],  the  imposed  condition  for  urban  driving  is  that  the  vehicle  does  not  contact 
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other  vehicles  if  the  other  vehicles  maintain  their  current  trajectory;  this  thesis  deals 
with  relaxing  such  an  assumption  about  the  dynamic  obstacles.  Additional  safe  states 
are  found  as  those  that  can  be  propagated  into  some  invariant  safe  state. 


4.2  Modified  Invariant  Safe  States 


For  the  scenario  described  earlier  in  Section  1.2,  once  the  infinite  horizon  velocity 
obstacle  sets  are  found,  any  control  input  v *  outside  of  VOS* (to,  oo)  for  all  obstacles  i 
can  be  propagated  indefinitely  without  possibility  of  collision,  simply  by  definition  of 
the  infinite  horizon  velocity  obstacle.  Thus,  moving  along  this  trajectory  defined 
by  v*  (the  dashed  arrow  in  Figure  4-1)  can  be  considered  an  invariant  safe  state, 
where  “state”  does  not  imply  a  specific  coordinate  or  time,  as  it  might  in  the  classic 
definition.  Instead,  assuming  the  host  robot  starts  at  the  origin  at  t  —  0,  the  “state” 
Sff*(t)  for  v *  is  defined  as 


Sy*  (t) 


[x(t)  —  tv' 

m] 


where  x(t),v(t)  are  respectively  the  host  vehicle’s  position  and  velocity  at  time  t. 
More  generally,  Sj?*(fi,f)  is  defined  as 


S V  *  (t  1 ,  t'} 


[£(f)  -  (£(ti)  +  (t-  h)v*)] 

Wit)} 


where  t  >  ti,  and  t]  is  the  time  of  computation  of  the  VOS. 

This  redefinition  allows  a  single  collision-free  state  to  remain  time-invariant  for  t  > 
t\.  Indeed,  if  the  vehicle  drives  at  constant  velocity  v*,  then  x(t)  =  x{t\)  +  (t  —  ti)v*, 
so 

Sv*  {tlit) 
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ds 

dt 


As  stated  earlier,  being  in  the  invariant  safe  state  s^*(t)  =  [0  0]  [u*]T  simply 

corresponds  to  the  robot  moving  with  the  guaranteed  safe  velocity  v*,  and  by  not 
changing  its  velocity,  the  robot  stays  in  this  state.  Note  that  every  time  the  infinite 
horizon  velocity  obstacle  sets  are  computed,  each  velocity  that  lies  outside  of  all  the 
VOSs  defines  a  new  invariant  safe  state  s$*(ti,t)  that  the  host  robot  may  choose  to 
transition  into.  Intuitively,  these  invariant  safe  states  are  escape  paths  (with  speed 
greater  than  the  forward  velocity  of  the  obstacle;  see  Section  3.3.2)  that  eventually 
leave  all  the  obstacles  behind  the  host  vehicle. 

Alternatively,  without  restructuring  the  definition  of  “state,”  there  would  be  no 
invariant  safe  states,  since  the  host  robot  needs  to  continue  moving  along  the  single- 
velocity  trajectory  to  ensure  collision  avoidance.  Nonetheless,  all  the  vehicle  configu¬ 
rations  along  this  trajectory  are  each  safe  states,  since  they  can  all  be  propagated  into 
other  collision  free  states  by  continuing  along  the  trajectory  at  velocity  v*.  The  above 
modification  to  the  definition  of  “state”  is  made  in  order  to  avoid  the  awkwardness 
of  defining  safe  states  without  having  any  invariant  safe  states  to  use  as  a  basis. 

4.3  Additional  Safe  States 

In  addition  to  the  invariant  safe  states  themselves,  any  vehicle  configuration  that  can 
safely  make  a  transition  into  a  invariant  safe  state  is  also  safe;  from  any  of  these  safe 
configurations,  it  is  always  possible  to  avoid  potential  collisions  simply  by  making  the 
transition  to  an  invariant  safe  state  and  staying  there  (i.e.,  continuing  with  velocity 
v*).  This  means  that  an  arbitrary  finite  trajectory  from  t  —  0  to  t  —  t±  (traji  in 
Figure  4-1)  terminating  with  the  conditions 

x  (ti)  =  x0  +  v*ti  (4.1) 
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Figure  4-1:  Different  safe  trajectories  in  physical  space,  velocity  obstacles  not  shown.  Let 
v*  be  a  velocity  outside  of  the  infinite  horizon  VOS  calculated  at  t  =  0.  The  dotted  arrow 
is  the  single- velocity  escape  trajectory  associated  with  v  * .  which  is  guaranteed  safe  on  an 
infinite  time  scale.  If  traji  terminates  at  (x\,  ti)  with  velocity  v*,  it  can  be  safely  continued 
at  least  along  traj2-  By  calculating  the  shifted  velocity  obstacle  at  (xi,t±),  other  single 
velocity  trajectories  (like  trajs)  that  also  guarantee  infinite  horizon  safety  may  be  found  to 
continue  any  partial  trajectory  ending  at  (a?i,ti). 


and 


v(ti)  =  v * 


(4.2) 


at  some  value  of  t\  >  to,  without  collisions  for  t  G  [0,  t\],  is  guaranteed  to  have  access 
to  a  trajectory  that  is  safe  on  the  interval  t  G  [t\,  oo).  If  to  in  the  VOS  computation  is 
set  to  essentially  0  (using  e  as  described  in  Section  3.3.1),  v *  itself  trivially  produces 
such  a  trajectory  for  t  G  [0, fi],  i.e. ,  in  Figure  4-1,  the  dotted  black  line  provides  a 
safe  means  of  arriving  to  the  blue  point  at  t\.  However,  various  more  complicated 
solutions,  like  the  dotted  green  line,  may  be  found  as  well.  Depending  on  the  specific 
scenario  and  host  robot  dynamics,  various  means,  including  forward  simulation  and 
prediction  as  in  [28]  and  [29],  can  be  used  to  generate  the  finite  trajectories  from  t  —  0 
to  t  —  t\  and  check  for  collisions.  The  velocity  obstacle  set  does  not  aid  in  checking 
the  safety  of  this  segment. 
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t=tl,T=  0 


Figure  4-2:  For  a  general  coordinate  x\  that  is  reached  by  the  host  robot  at  time  t\ ,  a 
time-shifted  VOS  can  be  computed  for  each  obstacle  that  determines  the  safety  of  single¬ 
velocity  trajectories  originating  from  x\  for  times  t  >  1 1.  It  is  convenient  to  define  a  new 
time-frame  T  =  t  —  t\  such  at  T  =  0,  that  the  host  robot  is  at  x\  and  the  reachable  set  of 
the  obstacle  has  already  grown  out  of  its  initial  location  by  t\  seconds.  The  time-shifted 
VOS  is  computed  the  same  way  as  detailed  in  Chapter  3  using  these  reachable  sets  for  all 
T  >  0. 

4.4  Time-shifted  VOSs 


Similarly,  but  more  generally,  for  any  finite  trajectory  that  terminates  at  time  t\  at 
an  arbitrary  position  X\  with  velocity  Vi,  the  terminal  location  X\  is  not  in  contact 
with  any  of  the  obstacles  if  it  is  not  in  the  reachability  set  SCRj(ti)  of  any  of  the 
obstacles.  If  this  condition  is  satisfied,  then  there  might  be  a  safe,  infinite  horizon 
trajectory  on  the  interval  t  e  [ti,  oo)  if  V\  lies  outside  of  every  time  shifted  VOS  with 
the  host  robot  moved  to  xj .  That  is,  assuming  the  robot  gets  to  x\  at  ti,  one  must 
determine  if  continuing  indefinitely  at  V\  is  guaranteed  safe  for  all  later  times. 

To  compute  the  time-shifted  VOS  of  each  obstacle,  a  new  time  frame  is  defined 
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using 


T  —  t  —  ti, 

such  that  T  measures  the  time  that  has  elapsed  since  the  host  robot  arrived  at  location 
X\  at  time  t\,  and  the  goal  is  to  find  velocities  leaving  X\  that  are  safe  for  T  >  0. 
Each  obstacle  is  translated  by  —x\  such  that  the  coordinates  are  defined  relative  to 
the  new  host  robot  location  at  T  =  0  (see  Figure  4-2).  In  addition,  when  computing 
the  reachable  sets  of  the  obstacles,  any  terms  T  are  replaced  by  T  + 1 1  to  account  for 
the  motion  of  the  obstacle  that  has  already  occurred  prior  to  T  =  0.  When  converting 
these  reachability  regions  defined  in  physical  to  velocity  space  regions,  the  factor  of 
j,  remains  unchanged.  Applying  these  changes  to  the  terms  in  the  equations  for  S{t) 
and  Q{t )  (Equation  2.7,  Equation  2.8,  Equation  2.11,  Equation  2.12,  Equation  3.2, 
and  Equation  3.3)  yields  the  boundaries  of  the  time-shifted  simplified  collision  regions 
SCR(T). 

For  example,  the  expression  for  Q2(9,t)  in  Equation  3.10  converts  to 

1  fay)  —  X\  +  p(l  —  cos 9)  +  (v(T  +  ti)  —  p9  +  r)  sin o\ 
Q2(9,T,x1,t1)  =  -  ,  (4.3) 

1  2/0  —  2/i  +  P sin#  +  (v(T  +  1 1)  —  p6  +  r)  cosd 

r  i T 

where,  as  before,  xo  yo  are  the  initial  coordinates  of  the  obstacle;  with  domain 

Q2(9,T,X i,ti)  :  T  e  [ t0,tf ],  9  e  [0,min(a;(T  +  fi),vr)] 

Using  the  steps  given  in  Section  3.2.1,  the  necessary  condition  for  boundary  points 
on  Q2(9,T,xi,ti)  Equation  3.12  becomes 

0  =  -  ^((x0  -xi  +  p)  sin  9  +  (y0-  yx)  cos  9  +  vtx  +  r  -  p9.  (4.4) 

Similar  modifications  can  be  performed  for  the  segments  <31,3,4,5(0,  T,  Xi,  U),  and  the 
necessary  conditions  for  boundary  points  on  each  of  these  segments  can  be  re-derived. 
If  the  velocities  obstacle  sets  are  calculated  using  these  modifications,  they  repre- 
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sent  all  the  potentially  dangerous  single-velocity  trajectories  that  originate  from  x\ 
at  t,\  ,  given  the  current  locations  of  all  the  obstacles.  In  an  equivalent  interpretation 
of  the  time-shifted  velocity  obstacle  sets,  these  are  VOSs  formed  for  the  current  time 
( T  =  0)  and  host  vehicle  location,  but  without  up  to  date  information  of  the  obstacle 
locations;  the  obstacle  state  is  known  for  T  =  —t\  but  has  since  changed. 

Any  velocity  that  lies  outside  of  the  time-shifted  VOS  can  be  executed  indefinitely 
without  the  possibility  of  collision  beyond  t  —  t\.  Hence,  any  finite  trajectory  termi¬ 
nating  at  an  arbitrary  X\  at  t\  can  be  safely  extended  if  the  terminal  velocity  hj  of 
this  trajectory  lies  outside  of  the  shifted  VOS  of  each  obstacle.  Note  that  the  con¬ 
ditions  described  earlier  in  Equation  4.1  and  Equation  4.2  are  a  specific  case  of  this 
condition;  v*  is  guaranteed  to  be  one,  among  possibly  many  others,  velocity  that  is 
outside  of  the  VOSs  calculated  at  (iq,ti).  In  Figure  4-1,  v*  gives  the  safe  trajectory 
traj2,  and  other  safe  trajectories  like  traj3  may  be  found  using  the  shifted  VOSs. 

The  time-shifted  VOS  can  be  used  in  on-line  planning  to  account  for  delays;  the 
planner  can  assign  a  duration  of  time  for  computation,  use  that  as  0 ,  and  set  x[  =  vt\ 
as  the  expected  propagation,  where  v  is  the  current  velocity.  Assuming  the  current 
velocity  is  maintained  during  planning,  this  formulation  properly  handles  on-board 
computation  time  and  does  not  compromise  guaranteed  collision  avoidance. 

The  time-shifted  VOS  also  has  potential  to  be  a  powerful  tool  for  efficient  path 
planning  while  maintaining  guaranteed  safety;  finding  safe  endpoints  of  general  finite 
trajectories  allows  for  much  smoother  planning  compared  to  picking  only  straight 
trajectories  using  the  various  guaranteed  safe  velocities.  This  will  be  discussed  in 
detail  in  Chapter  5. 
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Chapter  5 


Iterative  Planning 

5.1  Underlying  Concepts 

A  trajectory  that  satisfies  the  velocity  obstacle  set  constraint  calculated  with  tf  —  oo 
for  all  the  obstacles  can  be  continued  indefinitely  without  collision.  However,  if  the 
goal  location  does  not  lie  along  the  current  path,  or  if  the  goal  location  is  changed, 
the  robot  would  never  reach  the  goal  without  picking  a  different  velocity,  and  the 
trajectory  needs  to  be  updated.  There  is  no  guarantee  that  it  is  possible  to  reach  the 
goal  safely. 

At  any  point  in  time,  the  planner  can  use  the  current  state  information  to  recom¬ 
pute  the  VOS  of  each  obstacle.  The  safety  guarantee  is  that,  if  the  current  trajectory 
had  satisfied  the  infinite  horizon  VOS  constraints  at  some  previous  time,  the  current 
velocity  will  still  lie  outside  of  all  the  updated  VOSs.  In  addition,  if  there  are  other 
velocities  that  satisfy  this  condition,  there  may  be  other  feasible  trajectories  that  are 
guaranteed  safe  as  well.  These  may  be  single- velocity  trajectories  using  a  valid  v*,  or 
more  complex  finite  paths  whose  terminal  conditions  satisfy  either  Equation  4.1  and 
Equation  4.2  or  the  more  general  requirements  given  by  the  time-shifted  VOSs  (Sec¬ 
tion  4.4).  Here,  feasibility  refers  to  scenario-dependent  constraints  on  the  host  robot’s 
dynamics;  the  robot  may  not  be  able  to  immediately  change  its  speed  and  heading  to 
match  the  desired  new  safe  velocity  (see  Section  5.2.2  for  a  detailed  discussion).  If  a 
significant  amount  of  time  or  displacement  is  required  to  make  the  transition,  either 
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a  finite  path  that  eventually  merges  onto  the  new  trajectory  can  be  computed  and 
checked  for  safety  (Section  4.3),  or  the  time-shifted  VOS  can  be  used. 

Typically,  after  the  VOSs  are  updated,  new  safe  trajectory  will  be  available,  in 
addition  to  the  guaranteed  safety  of  the  current  trajectory.  Clearly,  for  any  safe 
velocity  pointed  in  the  direction  of  the  goal,  if  the  distance  to  the  goal  divided  by 
the  speed  is  less  than  the  time  horizon  of  the  VOSs,  the  host  robot  will  safely  reach 
the  way-point.  If  there  are  no  such  velocities,  a  safe  velocity  can  be  chosen  subject 
to  various  heuristics.  For  example,  to  choose  the  trajectory  that  makes  the  closest 
approach  to  the  desired  way-point  at  some  future  time,  the  velocity  whose  direction 
is  most  closely  aligned  with  the  goal  should  be  chosen.  Alternatively,  candidate  safe 
velocities  can  be  propagated  for  the  duration  of  time  until  the  next  scheduled  re¬ 
plan,  and  a  best  velocity  can  be  chosen  based  on  the  resulting  robot  configuration. 
For  example,  the  planner  could  greedily  select  the  propagated  location  closest  to  the 
goal,  or  it  could  also  incorporate  a  measure  of  how  sharply  the  robot  would  need 
to  turn  to  travel  from  the  propagated  location  to  the  desired  goal.  Other  selection 
metrics  could  include  measures  of  control  effort. 

At  any  later  time,  it  is  always  possible  to  recompute  the  VOSs  and  attempt  to 
improve  the  trajectory.  Re-planning  can  occur  on  regular  intervals,  or  it  can  be 
triggered  by  how  well  the  current  plan  approaches  the  goal.  A  better  trajectory  may 
not  exist,  and  there  is  no  guarantee  that  the  goal  can  ever  be  reached,  but  at  least 
the  current  plan  can  be  executed  safely  and  extended  indefinitely  (if  the  VOSs  were 
computed  for  the  infinite  time  horizon).  Therefore,  after  solving  for  the  velocity 
obstacle  sets  given  the  initial  state  of  the  environment,  if  there  exists  at  least  one 
feasible  velocity  v *  outside  of  all  the  infinite  horizon  VOSs,  an  iterative  planner  is 
guaranteed  to  never  encounter  collisions.  Note  that  the  safety  guarantee  does 
not  depend  in  any  way  on  the  iterative  updates,  which  only  help  the  robot 
seek  the  way-point.  Other  receding  horizon  methods  ([2,  9])  have  not  been  able  to 
provide  this  guarantee  for  unpredictable  obstacles. 

This  approach  is  not  complete ;  velocity  obstacle  sets  only  return  single- velocity 
escape  trajectories.  There  may  exist  a  curved  path  that  extracts  the  host  robot  from 
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a  “surrounded”  situation  or  one  that  brings  it  to  the  goal,  but  the  method  presented 
in  this  thesis  will  not  find  it.  However,  this  method  does  guarantee  that  it  will  never 
pnt  the  robot  in  such  a  surrounded  situation  if  that  is  not  the  initial  condition. 

5.2  Outline  of  Planners  Used  in  the  Simulations 

An  iterative  planner  based  on  the  concepts  discussed  in  Section  5.1  was  implemented 
in  simulation  for  scenarios  with  various  conditions  to  demonstrate  the  use  of  velocity 
obstacle  sets  in  avoiding  unpredictable,  dynamic  obstacles.  Multiple  obstacles  are 
seeded  throughout  then  environment  and  propagated  in  accordance  with  the  assumed 
dynamical  model.  The  host  vehicle  is  given  a  series  of  way-points  it  attempts  to 
reach,  if  it  can  do  so  while  avoiding  all  the  obstacles.  The  planner  is  provided  perfect 
knowledge  of  the  obstacles’  model  parameters  (size,  speed,  and  turn  radius)  and 
current  locations  and  headings  such  that  it  can  compute  the  velocity  obstacle  sets, 
but  it  is  given  no  additional  information.  On  regular  intervals,  the  planner  computes 
the  VOSs  of  each  obstacle  and  uses  a  greedy  heuristic  to  choose  and  immediately 
implement  a  safe,  dynamically  feasible  velocity  that  brings  it  towards  the  next  way- 
point.  The  key  result  of  this  thesis  is  that,  by  computing  the  infinite  horizon 
velocity  obstacles,  the  host  robot  can  successfully  avoid  the  obstacles  ad 
infinitum  while  attempting  the  navigate  to  the  way-points.  Note  that  just 
reaching  the  goal  is  not  considered  safe;  the  robot  must  maintain  an  open  path  forward 
from  there.  This  allows  the  simulation  to  run  indefinitely  without  the  robot  ever 
colliding. 

The  infinite  horizon  planner  is  tested  without  imposing  constraints  on  the  host 
vehicle,  and  then  tested  again  with  turn-rate  restrictions  on  the  host  vehicle,  both 
cases  agreeing  with  the  analytically  proven  safety  guarantee.  Then,  to  examine  the 
trade-off  between  long-term  safety  and  short-term  goal-seeking  and  to  demonstrate 
the  capabilities  of  the  VOS  formulation  as  a  reactive  planner,  finite-horizon  VOSs 
are  used  in  place  of  the  infinite  horizon  VOSs,  and  the  simulations  are  re-run  using 
various  horizon  lengths  for  both  cases  where  the  host  robot  has  unconstrained  or 
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constrained  dynamics.  Finally,  in  certain  applications,  the  traversable  environment 
may  have  enclosing  boundaries  (walls,  or  limits  of  explored  terrain),  in  which  case 
any  non- zero  velocity  would  lead  to  a  collision  on  the  infinite  horizon.  Various  length 
finite-horizon  planners  with  no  long-term  safety  guarantees  are  tested  in  simulation 
for  this  case  and  the  empirical  results  are  compared. 

5.2.1  Implementation  Details  of  the  Simulator 

This  section  describes  in  detail  the  implementation  of  the  iterative  planning  algo¬ 
rithms  in  outlined  in  Section  5.2. 

Basic  Parameters  In  these  simulations,  six  obstacles  move  with  speed  1  m/s  and 
turn  at  a  maximum  rate  of  7r/5rad/s,  yielding  an  equivalent  minimum  turning  radius  of 
p  fa  1.59  m.  The  host  robot  has  a  maximum  speed  of  vmax  =  2.5m/s.  In  the  simulations 
for  which  the  host  robot  has  a  limited  turn  rate,  it  may  adjust  its  heading  immediately 
by  up  to  Ad  =  y/aradians  at  each  planning  update.  The  dynamics  are  propagated 
linearly  at  intervals  of  St  =  0.1s  while  the  planner  makes  updates  every  At  =  Is,  so 
the  limited  instantaneous  heading  adjustment  translates  into  an  equivalent  turn  rate 
of  between  n/'s  and  107r/3  rad/s  (see  Section  5.2.2  for  a  detailed  discussion  on  how  this  is 
obtained).  The  host  robot  and  the  obstacles  are  all  disks  of  radius  0.5m,  yielding  a 
collision  radius  r  =  lm.  Each  simulation  runs  for  a  total  duration  of  800s  of  virtual 
time. 

Obstacle  behavior  To  represent  all  feasible  obstacle  behavior,  obstacle  turn  rates 
are  uniformly  sampled  from  their  full  range  of  [— oj,uj\  every  one  to  two  seconds.  If 
new  turn-rates  were  sampled  too  often,  the  trajectories  would  become  noisy  -  but 
mostly  straight  -  lines,  which  would  not  present  a  challenging  collision-avoidance 
scenario.  To  keep  them  clustered  in  a  local  area  and  to  sufficiently  test  the  limiting 
cases,  the  obstacles  are  made  to  turn  at  maximum  rate  whenever  they  are  outside 
of  a  pre-dehned  122m2  box  (the  dashed  lines  in  Figures  5-2-5-10)  in  at  the  center  of 
the  environment.  For  the  simulations  in  which  the  space  is  enclosed  by  walls,  the 
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walls  form  a  19.42m2  centered  on  the  origin  (in  the  figures,  the  walls  are  the  limits 
of  the  viewing  window),  such  that  given  the  above  described  obstacle  behavior,  the 
walls  mark  the  edge  of  the  obstacles’  reachability.  Collisions  between  obstacles  do 
not  affect  their  motion  so  as  not  to  violate  the  assumed  dynamics. 

Goal-seeking  and  collision  avoidance  for  host  robot  The  host  robot  is  re¬ 
peatedly  given  the  way-points  (0,-6),  (6,0),  (0,6),  and  (—6,0)  in  sequence,  such 
that  it  is  continuously  led  in  a  counter-clockwise  circuit  through  the  region  occupied 
by  the  moving  obstacles.  Whenever  the  host  robot  successfully  gets  within  0.1m  of 
the  current  way-point,  the  next  way-point  is  given  to  the  robot.  At  each  planning 
iteration  occurring  at  intervals  of  AT  =  Is,  the  VOS  of  time  horizon  r  for  each  ob¬ 
stacle  is  computed  according  to  Algorithm  1.  The  velocities  that  lie  outside  of  each 
VOSi(e,T )  yield  single-velocity  trajectories  that  are  guaranteed  to  be  collision-free 
with  respect  to  the  obstacles  for  the  desired  time  horizon.  If  walls  are  defined,  the 
velocities  are  again  filtered  such  that  only  the  those  do  not  reach  the  walls  by  time 
t  —  t  are  considered  safe.  Of  these  safe  trajectories,  the  dynamically  feasible  ones 
are  those  that  are  less  than  vmax  in  magnitude  and,  if  the  host  robot  is  described 
with  a  limited  turn-rate,  within  A 9  of  the  previous  velocity.  Note  that  the  dynamic 
constraints  of  the  host  robot  and  the  collision  avoidance  conditions  are  completely 
decoupled;  the  host  robot’s  dynamics  need  not  be  known  to  compute  the  VOSs,  so 
this  collision  avoidance  algorithm  can  be  used  modularly  in  various  scenarios. 

Dynamically  feasible  single- velocity  trajectories  that  are  guaranteed  collision-free 
up  to  time  r  are  ranked  using  a  simple  heuristic:  each  candidate  safe  trajectory  is 
propagated  for  a  duration  of  At,  and  the  velocity  that  brings  the  robot  the  closest 
to  the  current  way-point  is  chosen.  This  greedy,  one-step  look-ahead  approach  is 
not  the  time-optimal  solution,  but  in  most  cases  it  is  a  simple  approximation  that 
effectively  brings  the  robot  quickly  to  the  goal.  As  one  would  intuitively  expect,  the 
shortcomings  of  this  heuristic  are  most  apparent  when  a  sharp  turn  is  required  to 
reach  the  way-point.  In  this  thesis,  the  main  focus  of  the  work  is  about  defining 
the  constraints  that  guarantee  collision  avoidance,  so  improved  formulations  of  more 
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effective  path  planners  are  left  for  future  work.  Such  planners  would  greatly  benefit 
from  allowing  general  motion  primitives  that  terminate  in  provably  safe  single- velocity 
trajectories  (see  Sections  4.3  and  4.4). 

As  a  matter  of  implementation,  candidate  safe  velocities  are  found  as  the  boundary 
points  of  each  VOS,  as  well  as  the  intersection  of  these  boundaries  with  any  other 
constraints  (max  host  speed,  host  turn  rate  limits,  collision  constraints  relative  to 
wall).  These  velocities  are  sorted  by  the  metric  described  above,  and  candidates  are 
then  sequentially  checked  to  see  that  the  velocity  indeed  satisfies  all  the  constraints 
(i.e.,  a  boundary  point  of  one  VOS  does  not  he  on  the  interior  of  a  different  VOS). 
When  a  valid  velocity  is  found,  it  is  randomly  perturbed  by  small  amounts  until  a 
nearly  velocity  is  found  that  is  not  right  on  the  boundary  and  falls  on  the  correct 
side  of  it,  such  that  potential  contact  with  obstacles  is  completely  avoided,  whereas 
a  boundary  point  may  allow  the  host  vehicle  to  just  graze  the  obstacle. 

In  the  case  that  no  dynamically  feasible  velocities  are  guaranteed  safe  for  the 
desired  horizon  r,  the  planner  implemented  here  simply  selects  the  dynamically  fea¬ 
sible  velocity  that  best  satisfies  this  goal-seeking  heuristic,  without  attempting  to 
quantify  the  level  of  risk  for  each  unsafe  trajectory  and  compute  a  trade-off  with  the 
expected  time-to-goal.  Note  that  this  situation  is  proven  to  never  arise  if  r  =  oo. 
The  authors  of  Ref.  [15]  explore  in  detail  how  this  can  be  done  for  velocity  obstacles 
of  objects  moving  along  known  trajectories.  Their  method  of  computing  an  expected 
time-to-collision  could  be  applied  to  regions  within  the  velocity  obstacle  set  and  used 
accordingly,  but  such  an  implementation  has  been  omitted  here,  as  the  primary  focus 
of  this  work  is  guaranteed  safety. 

Differences  for  baseline  simulation  The  first  set  of  simulation  results  presented 
in  Section  5.3  focuses  on  the  baseline  characteristics  of  using  infinite  horizon  VOSs  to 
guarantee  collision  avoidance  with  no  dynamic  constraints  nor  enclosing  walls,  and 
the  simulation  is  implemented  with  some  minor  differences  in  the  details  described 
above.  There  are  only  four  instead  of  six  obstacles,  such  that  the  ensuing  behavior 
is  easier  to  understand.  Instead  of  being  given  a  fixed  sequence  of  way-points,  the 
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way-points  are  generated  randomly,  with  a  biased  to  be  close  to  the  current  location 
of  the  obstacles  in  order  to  create  interesting  collision  avoidance  scenarios.  The  best 
safe  velocity  is  selected  using  a  different  heuristic:  the  velocity  most  closely  aligned 
with  the  direction  of  the  goal  is  chosen  such  that  the  resulting  trajectory  is  the  one 
that  gets  the  closest  to  the  goal.  If  multiple  trajectories  point  directly  to  the  goal, 
the  one  with  speed  closest  to  1.5m/s  is  chosen  such  that  the  host  robot  is  not  always 
operating  at  its  dynamical  limits. 

5.2.2  Nonlinear  motion  of  the  host  robot 

The  core  concept  behind  hireling  velocity  obstacle  sets  for  collision  avoidance  is  that 
these  boundaries  in  velocity  space  separate  the  safe  single-velocity  trajectories  from 
the  potentially  dangerous  ones.  However,  there  is  no  simple  modification  to  adapt  it 
to  immediately  determine  the  safety  of  general,  nonlinear  motion  of  the  host  robot. 
Instead,  a  nonlinear  trajectory  would  need  to  be  decomposed  into  general  segments 
and  straight,  single-velocity  components.  Safety  with  respect  to  the  unpredictable 
dynamic  obstacle  is  determined  by  checking  that,  at  all  times,  the  robot  position  lies 
outside  of  the  simplified  collision  region  SCR(f)  (Section  2.1)  of  each  moving  obstacle. 
For  any  segment  of  a  general  trajectory,  this  check  of  physical  space  constraints  can  be 
done  in  a  straight-forward  manner  by  systematically  sampling  times  in  the  window  of 
interest  and  computing  the  collisions  regions.  For  single- velocity  segments  defined  for 
some  interval  [t\,  £2]  (where  f2  may  be  set  to  00),  this  check  may  be  simpler  to  perform 
by  computing  VOS(fi,f2).  Note  that  if  ti  ^  0,  the  VOS  needs  to  be  calculated  as  a 
time-shifted  VOS  (Section  4.4). 

Therefore,  to  properly  handle  the  non-instantaneous  turning  of  the  host  robot, 
the  motion  should  be  broken  down  as  a  finite  curved  segment  up  to  some  time  t\ 
terminating  at  some  known  location  x\,  followed  by  a  single- velocity  trajectory  out 
to  infinity.  The  safety  up  to  t\  needs  to  be  checked  in  physical  space,  and  ensuing 
safety  out  to  tf  =  00  is  guaranteed  if  the  final  velocity  is  outside  of  the  shifted  VOS 
found  using  t\  and  X\ .  This  would  allow  the  planner  to  consider  much  more  general 
paths,  if  there  were  a  more  sophisticated  algorithm  in  place  to  generate  and  rank  such 
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Figure  5-1:  Nonlinear  piece-wise  motion  of  the  host  robot.  If  the  robot  has  unlimited  turn 
rate,  the  motion  can  be  accurately  represented  by  the  dotted  black  line.  If  not,  the  velocity 
obstacle  set  calculations  do  not  directly  aid  in  determining  the  safety  along  any  curved 
segments,  which  need  to  be  checked  in  physical  space  over  whatever  times  they  are  defined 
to  be  technically  correct.  Alternatively,  in  discrete  simulation  time,  the  red  trajectory  is 
equivalent  to  the  dotted  black  trajectory,  such  that  curvature  constraints  of  the  red  curve 
can  be  translated  into  angular  constraints  at  the  corner  in  the  linear  approximation.  The  de- 
facto  turn-rate  limit  of  the  robot  (the  green  curve)  represented  by  this  linear  approximation 
depends  on  the  re-plan  frequency  and  is  less  sharp. 


trajectories. 

The  added  complexity  of  this  formal  approach  can  be  bypassed,  however,  if  the  ex¬ 
act  nonlinear  trajectory  of  turning  for  a  duration  of  5t  and  then  proceeding  straight 
can  be  closely  approximated  by  an  instantaneous  change  of  direction  followed  by 
single-velocity  motion  (See  Figure  5-1,  equivalent  exact  trajectory  in  red,  instanta¬ 
neous  change  in  dotted  black).  In  a  discrete-time  simulation  with  time  step  8t,  they 
are  indeed  identical  if  the  instantaneous  change  in  heading  A 9  in  the  piece-wise  lin¬ 
ear  trajectory  is  less  than  the  actual  turn  rate  ny,  of  the  host  robot  multiplied  by 
5t.  Hence,  as  described  in  Section  5.2.1,  limiting  the  choice  of  new  velocities  at  each 
re-plan  to  those  within  A 6  <  u)^5t  equivalently  represents  a  limited  turn  rate  of  the 
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host  robot  that  is  less  than  u;max  =  Ae)/<5 t  rad/s,  if  the  actual  motion  is  structured  as 
quick  turns  of  duration  5t  followed  by  straight  motion. 

Alternatively,  without  being  technically  identical,  restricting  the  instantaneous 
change  of  heading  to  be  at  most  A 6  can  be  treated  as  an  equivalent  maximum  turn 
rate  of  camax  =  a8/a t  rad/s  of  the  host  vehicle,  where  At  is  the  time  between  successive 
re-plans.  That  is,  given  that  the  host  vehicle  can  only  change  its  velocity  one  re-plan 
at  a  time,  the  overall  rate  of  angular  change  of  the  trajectory  over  the  course  of  several 
re-plans  (green  curve  in  Figure  5-1)  is  limited  by  the  product  At  A 9.  However,  it  is  not 
technically  accurate  to  say  that  if  the  host  robot  has  dynamics  limited  by  t^h  <  A8/a t 
it  would  be  able  to  follow  the  piece-wise  linear  trajectory,  which  is  implemented  in 
the  simulator,  down  to  discrete-time  accuracy.  Instead,  a  vehicle  with  such  dynamics 
would  be  able  to  use  the  piece-wise  linear  trajectory  as  a  reference  trajectory,  where 
the  actual  path  traversed  would  consist  of  curved  segments  merging  onto  the  reference 
trajectory  (see  Section  4.3)  after  At  seconds.  The  safety  along  the  curved  segments 
would  need  to  be  checked  by  other  means,  or  simply  assumed. 

For  the  purposes  in  this  thesis,  the  distinction  between  the  two  interpretations  is 
not  very  important.  For  smoother  path  planning  that  is  capable  of  considering  more 
general  escape  trajectories  than  just  the  single-velocity  ones,  it  will  be  worthwhile 
to  have  an  implementation  capable  of  checking  curved  segments  followed  by  infinite 
horizon  guarantees  provided  by  time-shifted  VOSs.  This  would  be  the  best  way  to 
properly  handle  realistic  turn-rate  constraints  of  the  host  vehicle,  but  fully  imple¬ 
menting  such  a  planner  still  requires  much  additional  work.  Until  then,  capturing 
the  qualitative  effects  of  non- instantaneous  turning  by  imposing  some  maximum  A 6 
is  sufficient  for  commenting  on  how  dynamic  limitations  affect  the  safety  and  the 
navigational  capabilities  of  the  planner  under  different  conditions. 

5.3  Baseline  Infinite  Horizon  Planner 

First,  the  simulation  is  run  using  infinite  horizon  VOSs  with  no  dynamic  constraints 
nor  enclosing  walls.  The  safety  guarantee  is  manifest  as  the  fact  that,  whenever  a 
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re-plan  occurs,  the  current  velocity  is  always  among  the  safe  velocities  found  using  the 
updated  VOSs,  since  the  current  single-velocity  trajectory  is  guaranteed  to  remain 
obstacle-free  for  all  time.  Indeed,  this  simulation  runs  for  the  full  duration  of  1000s  of 
virtual  time  without  the  robot  colliding  with  any  obstacles,  and  select  screen-shots  are 
shown  in  the  following  figures  to  illustrate  the  resulting  collision  avoidance  behavior. 

5.3.1  Interpretation  of  Figures 

In  the  ensuing  figures,  the  physical  space  is  shown  on  the  left.  The  host  vehicle  is  in 
blue,  the  obstacles  are  in  magenta,  and  the  way-point  is  the  red  star.  The  vehicles 
leave  a  trail  of  their  most  recent  positions.  The  trail  of  the  host  vehicle  is  reset  when 
it  reaches  a  way-point  and  is  assigned  a  new  goal.  Whenever  the  obstacles  cross  the 
dashed  boundary,  they  are  forced  to  turn  around  (explained  in  Section  5.2.1). 

On  the  right  is  the  velocity  space.  The  infinite  horizon  VOS  of  each  of  the  four 
obstacles  is  shaded  in.  The  maximum  velocity  vmax  of  the  robot  is  represented  by  the 
black  circle.  The  ideal  velocity  (directly  aligned  with  the  way-point,  magnitude  equal 
to  preferred  speed)  is  the  green  vector;  the  best  new  safe  velocity  is  the  blue  vector; 
and  the  velocity  before  the  re-plan  is  the  dashed  black  vector.  The  safety  guarantee  is 
that,  until  the  host  vehicle  begins  traveling  along  a  different  velocity,  the  old  velocity 
will  still  lie  outside  of  all  the  VOSs  when  the  obstacle  states  are  updated.  Just  for 
directional  reference,  the  nearest  obstacles  are  sketched  in  dashed  outlines,  though 
they  cannot  be  truly  represented  in  velocity  space. 

5.3.2  Some  Sample  Snap-shots 

Visualizations  from  various  instants  in  time  are  presented  in  Figures  5-2  to  5-10.  The 
planner  makes  a  scheduled  update  every  At  =  Is,  but  the  snap-shots  may  also  fall  on 
non-integer  times  since  an  update  is  immediately  computed  whenever  the  host  robot 
reaches  a  way-point. 
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Figure  5-2:  Initial  condition  of  baseline  simulation.  Four  obstacles  are  scattered  in  the 
environment,  and  the  host  vehicle  is  given  a  random  way-point.  The  velocity  obstacles 
show  which  velocities  could  potentially  result  in  collisions  if  propagated  indefinitely.  Since 
there  is  no  safe  velocity  directed  straight  towards  the  goal,  the  planner  chooses  the  most- 
closely  aligned  safe  velocity.  Re-plans  in  the  future  may  adjust  this  trajectory  to  intersect 
the  desired  way-point.  Notice  that  the  closest  obstacle  blocks  off  the  largest  section  of 
velocity  space. 

5.4  Performance  under  Various  Conditions 


This  section  will  examine  the  changes  in  avoidance  behavior  and  the  trade-offs  be¬ 
tween  long-term  safety  guarantees  and  short-term  effectiveness  of  path  planning  as 
the  time-horizon  used  in  the  planner  is  varied  under  different  scenarios.  In  the  first 
batch  of  test  scenarios,  the  environment  is  not  enclosed  by  walls,  and  the  host  robot 
can  either  turn  arbitrarily  quickly,  or  it  is  held  to  a  limited  turn-rate.  For  each  case 
of  host  robot  dynamics,  multiple  simulations  are  run  using  a  spectrum  of  finite  time- 
horizons  for  the  VOSs  in  addition  to  the  infinite  horizon  formulation.  The  minimum 
finite  time-horizon  tested  is  At  =  Is,  for  only  avoiding  collisions  that  would  occur  on 
a  horizon  shorter  than  the  time  until  the  next  plan  would  obviously  do  a  poor  job 
at  avoiding  collisions.  As  the  finite  time  horizons  become  large  enough,  there  is  no 
discernible  difference  between  the  finite  and  infinite  horizon  cases. 

A  second  batch  of  simulations  for  which  the  space  is  enclosed  on  all  four  sides  by 
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Figure  5-3:  At  t  =  5.5s,  a  velocity  pointed  directly  towards  the  goal  is  guaranteed  safe,  and 
this  new  velocity  is  selected. 


Figure  5-4:  By  the  next  planning  iteration,  the  slightly  slower  ideal  velocity  had  become 
become  safe,  and  the  host  robot  adjusts  accordingly.  This  change  comes  from  the  two  left¬ 
most  obstacles  having  each  turned  away  from  the  robot’s  intended  path  while  it  had  been 
dynamically  feasible  for  them  to  turn  in  the  other  direction. 
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Physical  space  at  t  =  1 1 .3 


Velocity  Obstacles  at  t  =  1 1 . 


Figure  5-5:  At  t  =  11.3s,  a  velocity  is  found  that  squeezes  between  the  two  velocity  obstacles 
sets  towards  the  left. 
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Figure  5-6:  Two  seconds  later,  the  updated  VOSs  show  that  this  direct  path  is  indeed  still 
safe,  and  the  actual  trajectory  of  the  left-most  obstacle  has  allowed  wider  margins  on  the 
host  robot’s  left  side  while  the  upper  obstacle  continues  to  threaten  collision  if  the  host 
robot  were  to  turn  slightly  to  the  right. 
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Figure  5-7:  In  the  few  planning  iterations  before  t  =  22.7s,  the  best  safe  trajectory  that  the 
planner  had  selected  would  have  brought  the  robot  too  far  the  left  of  the  goal.  At  t  =  22.7s, 
the  updated  VOSs  show  that  there  is  now  a  safe  trajectory  nearly  lined  up  with  the  goal 
that  jointly  avoids  all  possible  trajectories  of  both  lower  obstacles. 
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Figure  5-8:  In  the  one  re-plan  between  t  =  22.7s  and  t  =  24.7s  the  robot  was  able  to 
make  one  more  minor  adjustment  in  its  heading  (as  seen  by  the  slight  deflection  of  the  blue 
trajectory),  lining  up  directly  with  the  goal. 
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Figure  5-9:  At  t  =  26.1s,  the  new  way-point  is  blocked  off  by  the  nearest  obstacle,  and  host 
robot  must  drive  around  it. 


Figure  5-10:  At  the  next  update,  the  planner  discovers  a  clear  trajectory  towards  to  goal. 
Note  that,  with  limited  turn  dynamics,  this  tight  turn  may  not  be  possible,  and  the  planner 
would  then  continue  along  the  previous  velocity  as  the  next  best  option. 
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walls  is  then  analyzed.  In  these  scenarios,  it  is  not  possible  to  form  infinite  horizon 
velocity  obstacle  sets,  since  any  single  velocity  trajectory  would  eventually  collide  with 
a  wall,  so  various  finite  time  horizons  are  tested  and  compared  for  both  constrained 
and  unconstrained  host  robot  dynamics.  Note  that  guaranteed  collision  avoidance 
requires  infinite  horizon  VOS  calculations  and  a  non-enclosed  environment. 

5.4.1  Measurements  Taken 

Measures  of  Safety  To  evaluate  the  safety  of  the  planner  in  each  condition,  the 
number  of  collisions  encountered  in  the  span  of  1000  seconds  of  simulation  is  recorded. 
These  collisions  are  either  due  to  planner  errors  or  VOS  numerical  errors. 

•  Planner  errors  These  occur  when  there  are  no  velocities  guaranteed  safe  for 
the  desired  time  horizon  r,  i.e.,  the  velocity  obstacle  sets  cover  all  of  the  dynam¬ 
ically  feasible  velocities  of  the  host  robot.  When  this  occurs,  the  planner  selects 
an  unsafe  velocity  that  best  satisfies  the  heuristic  for  way-point  navigation  (see 
Section  5.2.1).  These  errors  are  guaranteed  to  never  occur  if  the  time  horizon 
is  set  to  oo  and  the  robot  starts  in  a  valid  initial  condition.  Limited  vehicle 
dynamics  do  not  affect  this  guarantee  because  simply  continuing  at  the  current 
safe  velocity  is  always  dynamically  feasible. 

•  VOS  numerical  errors  The  circumstantial  geometry  of  the  boundary  of  the 
VOS  of  a  certain  obstacle  may  occasionally  be  problematic  for  the  algorithm 
to  process.  Two  boundary  points  may  be  too  close  to  cleanly  distinguish  nu¬ 
merically,  or  multiple  candidate  boundary  segments  may  overlap  in  a  very  near 
parallel  manner  that  makes  it  problematic  to  find  the  overall  outer  boundary. 
In  these  cases,  the  VOS  is  not  properly  computed,  and  collisions  may  result. 
Empirically,  the  VOS  calculation  fails  less  than  0.5%  of  the  time,  and  this  rate 
can  perhaps  be  reduced  with  further  refinement  in  the  coding  of  the  algorithm. 
Errors  of  this  type  should  be  disregarded  when  evaluating  the  effectiveness  of 
the  collision  avoidance  algorithm  under  various  conditions. 


These  errors  do  not  always  result  in  actual  collisions,  since  the  obstacles  may  end  up 
moving  along  relatively  benign  trajectory  among  all  its  dynamically  feasible  choices. 
Therefore,  the  error  rates  and  collision  counts  are  recorded  separately. 

Measures  of  Effective  Path  Planning  The  direct  distance  between  successive 
way-points  is  6v^2  «  8.5  meters,  so  by  traveling  at  the  maximum  speed  and  ignoring 
all  obstacles,  the  host  robot  would  need  ~  3.4  seconds  to  travel  from  way-point  to  way- 
point.  Accounting  for  the  collision  avoidance  constraints  in  the  form  of  the  velocity 
obstacle  sets  forces  the  host  robot  to  divert  from  this  direct  path  and  interferes  with 
the  goal-seeking  process.  The  average  time  it  actually  takes  for  the  robot  to  reach 
the  way-point  in  each  simulation  is  recorded  as  a  measure  how  effectively  of  the 
path-planner  can  navigate  the  way-points  while  operating  within  the  various  imposed 
safety  constraints. 

Empirically,  when  a  way-point  is  not  reached  immediately,  the  host  robot  typically 
needs  to  circle  around  the  environment  before  it  can  make  another  pass  to  the  same 
way-point.  It  is  also  fairly  common  for  multiple  obstacles  to  linger  in  the  vicinity  of 
a  way-point  such  that  the  robot  must  wait  a  significant  duration  before  it  is  possible 
to  reach  safely  approach  the  goal.  These  situations  give  rise  to  significant  outliers  in 
the  time-to-goal  data.  The  standard  deviation  is  be  calculated,  but  the  distribution 
is  not  normal,  so  its  characteristics  are  not  summarily  represented  by  the  average  and 
standard  deviation  alone.  Therefore,  a  distribution  of  times  is  also  recorded  for  each 
simulation;  the  fraction  of  paths  that  are  completed  within  various  multiples  of  the 
direct  path  time  3.4s)  is  recorded. 

Infinite  Horizon  Data  In  the  figures  in  which  these  measurements  are  plotted 
(Figures  5-14-5-17  and  5-21-5-24),  the  infinite  horizon  simulation  is  in  the  cases  with¬ 
out  walls,  and  the  data  for  r  =  oo  is  plotted  at  some  finite  value  (24  or  30)  at  the  far 
right  of  the  x  axis. 
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5.4.2  Results:  No  walls,  No  dynamic  constraints 

In  this  batch  of  tests,  like  the  case  in  the  baseline  implementation  of  Section  5.3,  the 
environment  is  not  enclosed  by  walls,  and  the  host  vehicle  can  turn  instantaneously 
to  any  new  velocity.  Figures  5-11  to  5-13  show  the  changes  in  the  VOS  constraints 
as  the  time  horizon  is  increased.  On  short  horizons,  only  the  obstacles  in  the  imme¬ 
diate  vicinity  of  the  host  robot  affect  the  safety  of  feasible  velocities.  The  planner 
essentially  acts  as  a  reactive  planner  and  does  not  make  use  of  the  VOS  formulation 
to  account  for  the  unpredictable  future  behaviors  of  the  obstacles.  Nonetheless,  the 
VOSs  still  do  accurately  represent  which  velocities  are  necessary  to  steer  away  from 
immediate  collisions,  and  as  long  as  the  host  robot  is  agile  enough  to  veer  away, 
collisions  can  generally  be  avoided.  Short  horizon  planners  could  be  vulnerable  to 
becoming  gradually  surrounded,  by  the  obstacles,  but  such  a  pathological  situation 
did  not  arise  during  the  simulation.  As  the  horizon  is  extended,  the  planner  becomes 
more  sensitive  to  possible  future  locations  of  the  obstacles,  and  would  be  susceptible 
to  encountering  situations  that  require  a  last  minute  dodging  behavior.  This  increased 
awareness  of  future  obstacle  trajectories  imposes  more  restrictive  limits  on  what  are 
considered  safe  velocities;  any  velocity  deemed  safe  on  a  longer  horizon  is  also  deemed 
safe  on  a  shorter  one.  Therefore,  in  general,  if  the  heuristic  for  selecting  velocities 
that  get  to  the  way-point  quickly  is  effective,  as  the  time  horizon  is  increased,  the 
average  time  to  goal  should  rise.  After  some  point,  further  extending  the  time  horizon 
has  negligible  effects  on  the  shape  of  the  VOSs,  so  the  performance  measures  should 
settle  as  r  — >  oo. 

Figures  5-14  to  5-17  confirm  these  trends.  With  an  unlimited  turn  rate,  the  host 
robot  does  fine  using  even  just  a  reactive  planner,  so  there  are  no  errors  or  collisions  on 
any  time  horizon  (the  VO  errors  are  numerical  issues,  as  explained  in  Section  5.4.1). 
As  the  time  horizon  is  increased,  the  average  time  to  goal  climbs.  There  is  a  minor 
peak  in  Figure  5-16  at  r  =  6s,  and  this  may  be  caused  by  transients  in  switching 
between  two  qualitatively  different  behaviors:  reacting  to  imminent  collisions  versus 
planning  more  roundabout  paths  using  an  essentially  infinite  horizon.  For  horizons 
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Figure  5-12:  Simulation  with  unconstrained  dynamics,  no  walls,  r  =  5s. 
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Figure  5-13:  Simulation  with  unconstrained  dynamics,  no  walls,  r  =  oo. 


greater  longer  10  seconds,  the  behavior  is  essentially  identical  to  the  infinite  horizon 
behavior.  With  an  unbounded  turn  rate,  if  the  host  robot  misses  the  way-point,  it 
can  immediately  turn  around  without  making  a  large  loop.  Therefore,  as  seen  in 
Figure  5-17,  there  are  not  very  many  extremely  long  paths. 
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Figure  5-15:  Collisions:  no  constraints,  no  walls 
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Figure  5-16:  Average  time  to  goal:  no  constraints,  no  walls 
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Figure  5-17:  Distribution  of  time  to  goal:  no  constraints,  no  walls 


92 


5.4.3  Results:  No  walls,  Dynamic  constraints 

In  this  batch  of  tests,  a  turn-rate  constraint  is  introduced  for  the  host  robot  such 
that,  at  each  planner  update,  it  may  only  choose  velocities  whose  angles  differ  by 
at  most  A 6  =  n/'s  from  the  previous  velocity  (see  section  5.2.2).  In  Figures  5-18  to 
5-20,  these  constraints  are  represented  by  the  red  bounds  (which  are  bisected  by  the 
previous  velocity,  the  dashed  black  vector):  only  velocities  from  within  the  wedge 
may  be  chosen  as  the  new  velocity. 

In  these  scenarios,  the  robot  is  less  agile,  so  reactive  motion  planning  is  naturally 
less  effective.  The  short-term  planner  may  attempt  a  direct  path  towards  the  way- 
point,  and  along  the  way,  it  then  discovers  a  threatening  obstacle  but  not  be  able 
to  swerve  avoid  it  in  time.  Such  situations  indeed  led  to  multiple  collisions  for  the 
Is  horizon  planner  (Figure  5-22),  and  threatened  other  collisions  that  were  avoided 
only  by  chance  (Figure  5-21).  This  issue  is  provably  averted  only  through  using  the 
infinite  horizon  planner,  though  in  practice,  the  likelihood  of  the  problem  drops  off 
sharply  as  longer  finite  horizons  are  used.  The  limited  dynamics  of  the  host  vehicle 
do  not  affect  the  infinite  horizon  safety  guarantee. 

Just  as  in  the  unconstrained  case,  if  the  velocity  selection  heuristic  is  effective, 
shorter  planning  horizons  would  generally  imply  shorter  times  to  goal,  since  there 
are  fewer  restricted  velocities.  However,  with  limited  turning  dynamics  on  the  host 
vehicle,  the  times  are  noisier  and  this  trend  is  less  pronounced.  Here,  the  heuristic 
will  often  fail;  the  vehicle  cannot  greedily  seek  the  goal  and  successfully  make  quick 
adjustments  along  the  way,  if  the  trajectory  is  deflected  by  an  obstacle,  the  host 
vehicle  needs  to  slowly  loop  back  around  before  its  again  aligned  with  the  way-point. 
Thus,  executing  a  plan  that  looks  deceptively  good  on  a  short  horizon  can  be  more 
costly,  and  these  competing  effects  give  rise  to  widely  scattered  data  in  Figure  5-23. 

All  together,  the  times  are  much  longer  in  this  scenario  compared  to  the  previous 
one.  This  comes  from  the  fact  that,  without  considering  more  general  safe  states 
and  multi-step  paths  (see  Sections  4.3  and  4.4),  the  planner  will  have  a  lot  of  trouble 
approaching  the  goal  with  vehicles  on  the  other  side  of  the  way-point,  due  to  the 
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Figure  5-19:  Simulation  with  constrained  dynamics,  no  walls,  r  =  5s. 


-5 


I  ’hysica  I  spare  at  t  —  2.9 

°e-  -  *  v 

©  o  6\  1 
©V 


-5 


VOS  with  r  -  oo  at  t  —  3 
3 


x  (m)  vx  (m/s) 

Figure  5-20:  Simulation  with  constrained  dynamics,  no  walls,  r  =  oo. 
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fact  that  the  linear  trajectory  must  be  continued  safely  (see  Figures  5-25  and  5-26). 
In  the  previous  scenario,  if  the  robot  misses  the  way-point  for  this  reason,  usually  it 
can  safely  immediately  double  back  to  reach  the  goal,  but  it  is  unable  to  do  so  with 
a  limited  turn-rate.  The  inability  to  take  such  a  corrective  action  causes  the  much 
longer  paths  (Figures  5-23  and  5-24).  This  effect  would  be  significantly  reduced  if  the 
planner  were  capable  of  considering  partial  trajectories  towards  the  goal  that  later 
along  safe  infinite  trajectories  in  a  different  direction;  even  the  dynamically  limited 
planner  would  be  able  to  have  a  higher  success  rate  on  the  first  passes  towards  the 
way-point. 
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Figure  5-22:  Collisions:  with  constraints,  no  walls 
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Figure  5-23:  Average  time  to  goal:  with  constraints,  no  walls 
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Figure  5-24:  Distribution  of  time  to  goal  with  constraints,  no  walls 
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Figure  5-25:  Were  the  planner  able  to  consider  more  general  trajectories,  it  could  find  a  safe 
path  that  goes  directly  to  the  goal  with  the  intention  of  later  merging  onto  a  safe  infinite 
trajectory  pointed  towards  the  right.  The  inability  to  consider  such  a  trajectory  hampers 
both  planners,  with  or  without  turn-rate  constraints. 
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Figure  5-26:  However,  without  turn  rate  constraints,  the  host  vehicle  would  have  been  able 
to  turn  back  immediately  to  take  the  green  trajectory.  In  this  case,  it  cannot  do  so,  and  it 
is  often  left  “pacing”  back  and  forth. 


5.4.4  Results:  Walls,  No  dynamic  constraints 

In  this  batch  of  tests,  the  environment  is  enclosed  by  walls  (at  the  limit  of  the  viewing 
window  of  the  left  sides  of  Figures  5-27  to  5-29),  and  the  host  vehicle  can  turn 
instantaneously  to  any  new  velocity.  The  velocities  that  would  result  in  a  collision 
with  the  wall  at  time  t  —  t  are  represented  by  the  red  boxes;  velocities  outside  of 
the  box  are  not  r-safe  and  are  not  chosen  by  the  planner.  Note  that,  with  multiple 
obstacles,  the  required  conditions  to  be  able  to  guarantee  collision  avoidance  are 
quite  complex.  In  a  pathological  scenario,  depending  on  the  size  of  the  room  and  the 
relative  dynamics  of  the  vehicles,  it  may  be  possible  for  the  obstacles  to  collectively 
herd  the  host  vehicle  into  a  corner  such  that  collision  is  inevitable.  This  situation 
does  not  arise  in  simulation. 

On  an  infinite  time  horizon,  any  non-zero  velocity  would  lead  to  a  collision  with 
a  wall,  so  the  infinite-horizon  planner  is  not  considered  here.  For  short  time  hori¬ 
zons,  unless  the  host  vehicle  is  very  close  to  a  wall,  the  reactive  planner  is  essentially 
unaffected.  As  the  time  horizon  is  increased,  avoiding  the  walls  becomes  an  increas¬ 
ingly  restrictive  requirement  that  essentially  limits  the  host  vehicle  to  lower  and  lower 
speeds.  This  effect  can  be  seen  in  the  left  halves  of  Figures  5-32  and  5-33.  Meanwhile, 
increasing  the  time  horizon  also  drives  each  velocity  obstacle  set  towards  cover  the 
entire  unit  circle  (as  explained  in  Section  3.3.2).  Past  roughly  r  =  10,  all  the  veloc¬ 
ities  within  the  red  box  imposed  by  the  walls  are  usually  covered  by  the  VOSs  such 
that,  more  often  than  not,  the  planner  fails  to  find  any  single-velocity  trajectories 
that  are  safe  for  duration  r  (Figure  5-30). 

As  explained  in  Section  5.2.1,  when  this  occurs,  the  planner  is  allowed  to  select 
an  arbitrary  velocity,  since  there  is  no  way  to  properly  satisfy  the  safety  constraint. 
By  r  =  16,  the  error  rate  reaches  100%  and  the  host  robot  is  driving  blindly;  it  heads 
straight  for  the  way-point  and  often  comes  in  contact  with  the  obstacles  (Figure  5-31. 
This  planner’s  performance  may  be  treated  as  the  “control”  case  where  nothing  is 
done. 

These  results  show  that,  when  operating  in  an  enclosed  environment,  it  does  not 
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figure  5-27:  Simulation  with  unconstrained  dynamics,  with  walls,  r  =  Is. 
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figure  5-28:  Simulation  with  unconstrained  dynamics,  with  walls,  r  =  5s. 
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Figure  5-29:  Simulation  with  unconstrained  dynamics,  with  walls,  r  =  10s. 
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make  sense  to  use  long  time  horizons,  since  no  single- velocity  trajectory  stays  safe  very 
long.  With  agile  dynamics,  short  term  planners  can  still  be  effectively  implemented 
to  reactively  dodge  obstacles  while  avoiding  swerving  into  the  wall. 
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Average  time  to  goal:  without  constrained  dynamics,  with  walls 


Figure  5-32:  Average  time  to  goal:  no  constraints,  with  walls 


Distribution  of  times  to  goal  compared  to  minimum  time:  without  constrained  dynamics,  with  walls 


103 


5.4.5  Results:  Walls,  Dynamic  constraints 

In  this  batch  of  tests,  the  environment  is  enclosed  by  walls,  and  the  host  vehicle  has 
a  limited  turn  rate.  In  Figures  5-34  to  5-36,  valid  velocities  must  lie  inside  both  the 
wedge  defined  by  the  limited  dynamics  and  the  box  defined  by  the  walls. 

As  one  would  expect,  this  batch  of  simulation  results  is  a  combination  of  the 
previous  two  cases.  Once  the  time  horizon  gets  too  long,  there  are  no  safe  trajectories, 
and  the  vehicle  reverts  to  driving  without  collision  avoidance.  This  transition  time 
horizon  occurs  a  little  earlier  than  it  did  without  dynamic  constraints,  since  the  added 
angular  restriction  increases  the  likelihood  that  no  safe  choices  are  available  (as  is  the 
case  in  Figure  5-36).  Before  the  transition  occurs,  the  turn-rate  restricted  planner  is 
not  nearly  as  effective  as  the  unconstrained  one,  both  in  terms  of  avoiding  collisions 
(Figure  5-37)  and  in  terms  of  planning  efficient  paths  to  the  way-points  (Figure  5-39). 
At  r  =  8s,  the  restricted  planner  is  on  average  faster,  but  this  is  due  to  the  switch  in 
behavior  occurring  earlier  at  r  =  6s. 
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Figure  5-34:  Simulation  with  constrained  dynamics,  with  walls,  r  =  Is. 


I  ’hysica  I  space  at  t  —  2.9  VOS  with  horizon  t  o  at  t  —  3 


x(m)  V  <Tl/3) 

Figure  5-35:  Simulation  with  constrained  dynamics,  with  walls,  r  =  5s. 


I  ’hysica  I  space  at  t  —  3.9  VOS  with  horizon  r  —  10  at  t  —  4 


x  (m)  v  <rn/s> 

Figure  5-36:  Simulation  with  constrained  dynamics,  with  walls,  r  =  10s. 
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Collisions:  with  constrained  dynamics,  with  walls 
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Average  time  to  goal:  with  constrained  dynamics,  with  walls 


Figure  5-39:  Average  time  to  goal:  with  constraints,  with  walls 


Distribution  of  times  to  goal  compared  to  minimum  time:  with  constrained  dynamics,  with  walls 
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Chapter  6 


Conclusions 


The  objective  of  this  thesis  is  to  develop  a  framework  for  guaranteeing  infinite  horizon 
collision  avoidance  of  unpredictable,  dynamically  constrained  moving  obstacles.  This 
goal  is  achieved  by  converting  the  reachable  sets  of  the  unpredictable  obstacles  into 
velocity  space  constraints,  whose  union  over  a  window  of  time  defines  the  velocity 
obstacle  set.  As  the  upper  limit  of  the  time  window  is  extended  to  infinity,  this  velocity 
obstacle  set  can  be  used  to  find  safe,  invariant  single- velocity  escape  trajectories  that 
can  be  used  in  a  receding  horizon  setting  to  provide  guaranteed  safety. 

6.1  Thesis  Summary 

The  objective  of  this  thesis  is  introduced  and  motivated  in  Chapter  1.  In  better  struc¬ 
tured  environments,  invariant  safe  states  are  often  used  as  a  cornerstone  in  proving 
safety  in  receding  horizon  settings,  but  such  states  have  been  difficult  to  define  in 
the  presence  of  external  agents  with  unpredictable  behavior.  Meanwhile,  velocity 
obstacles  are  widely  used  to  concisely  represent  conditions  for  guaranteed  avoidance 
of  predictable  obstacles.  However,  these  approaches  are  heavily  dependent  on  hav¬ 
ing  accurate  information  about  the  obstacles’  future  locations,  which  is  typically  an 
invalid  assumption.  The  algorithm  developed  in  this  thesis  does  not  require  such  an 
assumption,  and  uses  the  velocity  obstacle  concept  to  define  invariant  safety  in  veloc¬ 
ity  space,  thereby  successfully  guaranteeing  infinite  horizon  safety  in  this  challenging 
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setting. 

In  Chapter  2,  parametric  equations  are  found  to  describe  the  boundaries  of  simpli¬ 
fied  collision  regions  that  enclose  all  potentially  dangerous  locations  with  respect  to 
each  obstacle  as  a  function  of  time.  In  Chapter  3,  these  collision  regions  are  mapped 
into  velocity  space,  and  the  union  of  the  resulting  simplified  velocity  regions  over  a 
time  interval  defines  the  velocity  obstacle  set.  Velocities  that  are  not  within  the  veloc¬ 
ity  obstacle  set  can  be  safely  propagated  along  single-velocity  trajectories,  avoiding 
all  potential  collision  locations  for  the  duration  of  the  interval.  It  is  shown  that  the 
velocity  obstacle  set  can  be  legitimately  defined  for  an  open  time  interval  that  extends 
to  +oo. 

Thus,  as  laid  out  in  Chapter  4,  the  infinite  horizon  velocity  obstacle  set  can 
be  used  to  define  invariant  safe  states  with  respect  to  the  unpredictable  dynamic 
obstacles  in  the  form  of  single-velocity  infinite  trajectories.  These  safe  velocities  can 
be  immediately  used  as  the  raw  building  blocks  of  a  rudimentary  iterative  planner  with 
infinite  horizon  guarantees,  or  they  can  be  further  refined  to  define  more  general  safe 
states  that  would  serve  as  the  endpoints  of  arbitrary  trajectories  which  could  then 
be  used  in  more  advanced  iterative  planners  that  also  preserve  guaranteed  infinite 
horizon  collision  avoidance. 

Finally,  the  performance  of  iterative  planners  using  velocity  obstacle  sets  of  var¬ 
ious  time  horizons  are  presented  in  Chapter  5.  Only  the  infinite  horizon  planner  is 
guaranteed  safe,  but  simulation  results  show  that,  if  the  host  robot  has  sufficient 
mobility,  short  horizon  velocity  obstacles  can  be  used  in  planners  that  essentially  use 
reactive  collision  avoidance.  Such  planners  also  tend  to  be  more  successful  in  finding 
direct  paths  to  the  goal.  Furthermore,  they  can  be  used  in  confined  environments  in 
which  long  horizon  planners  are  invalid.  However,  the  short  horizon  planners  exhibit 
non-zero  error  rates,  and  are  notably  less  effective  and  more  risky  when  the  host 
robot’s  dynamics  are  limited.  The  simulation  results  also  suggest  that  performance 
in  all  cases  can  be  drastically  improved  with  generalized  safe  states  that  would  allow 
for  more  intelligent  trajectory  construction. 
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6.2  Future  work 


The  concept  of  Velocity  Obstacle  Sets  and  the  resulting  path  planning  algorithm 
can  be  expanded  and  improved  in  several  directions  with  additional  research.  All 
the  derivations  presented  in  this  thesis  apply  to  obstacles  with  assumed  unicycle 
dynamics,  i.e. ,  the  obstacles  move  with  constant  forward  speed  and  have  a  bounded 
turn-rate.  Additional  work  can  be  done  to  extend  the  result  to  obstacles  defined  by  a 
different  set  of  dynamics.  The  reachable  set  for  objects  with  a  maximum  speed  instead 
of  a  single  constant  speed  should  look  quite  similar  to  the  reachable  sets  discussed 
here,  so  an  extension  to  cover  this  more  general  case  may  not  be  too  difficult  to  derive. 
It  may  also  be  worthwhile  to  consider  the  problem  more  carefully  from  the  parallel 
perspective  of  inevitable  collision  states  (Section  2.3.1). 

In  computing  the  boundary  of  the  velocity  obstacle  set,  the  process  of  combining 
various  candidate  points  together  into  a  continuous  curve  is  not  yet  completely  robust. 
It  relies  heavily  on  basic  functions  that  check  for  intersections  between  arbitrary  line 
segments,  but  this  procedure  can  break  down  when  the  input  line  segments  extremely 
close  and  are  nearly  end  to  end,  parallel,  or  just  barely  overlapping.  In  the  current 
implementation,  these  issues  crucially  interfere  with  the  computation  of  the  boundary 
in  about  0.3%  of  the  attempted  computations.  A  careful  review  of  this  process  or 
an  attempt  to  find  an  alternative  means  of  processing  the  candidate  boundary  points 
may  reduce  or  eliminate  this  numerical  issue. 

Furthermore,  in  implementation,  this  is  the  most  computationally  intensive  step. 
It  can  require  up  to  half  a  second  (though  on  average  much  less)  to  process  a  boundary 
of  a  few  hundred  points  on  a  desktop  computer,  whereas  all  the  other  steps  combined 
require  a  fraction  of  that  time.  Conceptually,  velocity  obstacle  sets  are  perfectly 
suited  for  use  in  real-time  on-board  planning  algorithms,  so  for  this  purpose  as  well, 
improving  the  above  described  process  would  be  a  worthy  pursuit. 

Finally,  Chapter  4  lays  out  the  process  for  finding  potential  generalized  safe  states 
using  time-shifted  velocity  obstacle  sets.  These  would  allow  iterative  planners  to 
properly  handle  the  unavoidable  host  robot  non-linearities  of  turning,  and  to  create 
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far  more  effective  plans  using  finite  trajectories  terminating  in  safe  states  that  have 
infinite  horizon  safe  solutions,  as  discussed  in  Chapter  5.  The  implementation  of  an 
algorithm  for  computing  the  boundary  of  the  time-shifted  velocity  obstacle  sets  would 
therefore  be  a  useful  future  project. 
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